pcl::CorrespondencesPtr ORPointCloud::correspondences(const ORPointCloud* model, const ORPointCloud* scene)
{
pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ());
pcl::KdTreeFLANN<DescriptorType> match_search;
match_search.setInputCloud (model->descriptors);
// For each scene keypoint descriptor, find nearest neighbor into the model keypoints descriptor cloud and add it to the correspondences vector.
for (size_t i = 0; i < scene->descriptors->size (); ++i)
{
std::vector<int> neigh_indices (1);
std::vector<float> neigh_sqr_dists (1);
if (!pcl_isfinite (scene->descriptors->at (i).descriptor[0])) //skipping NaNs
{
continue;
}
int found_neighs = match_search.nearestKSearch (scene->descriptors->at (i), 1, neigh_indices, neigh_sqr_dists);
if(found_neighs == 1 && neigh_sqr_dists[0] < 0.25f) // add match only if the squared descriptor distance is less than 0.25 (SHOT descriptor distances are between 0 and 1 by design)
{
pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
model_scene_corrs->push_back(corr);
}
}
return model_scene_corrs;
}
int
main (int argc,
char* argv[])
{
//======== 【1】命令行参数解析=================================
parseCommandLine (argc, argv);
//======== 【2】新建必要的 指针变量============================
pcl::PointCloud<PointType>::Ptr model (new pcl::PointCloud<PointType> ());//模型点云
pcl::PointCloud<PointType>::Ptr model_keypoints (new pcl::PointCloud<PointType> ());//模型点云的关键点 点云
pcl::PointCloud<PointType>::Ptr scene (new pcl::PointCloud<PointType> ());//场景点云
pcl::PointCloud<PointType>::Ptr scene_keypoints (new pcl::PointCloud<PointType> ());//场景点云的 关键点 点云
pcl::PointCloud<NormalType>::Ptr model_normals (new pcl::PointCloud<NormalType> ());//模型点云的 法线向量
pcl::PointCloud<NormalType>::Ptr scene_normals (new pcl::PointCloud<NormalType> ());//场景点云的 法线向量
pcl::PointCloud<DescriptorType>::Ptr model_descriptors (new pcl::PointCloud<DescriptorType> ());//模型点云 特征点的 特征描述子
pcl::PointCloud<DescriptorType>::Ptr scene_descriptors (new pcl::PointCloud<DescriptorType> ());//场景点云 特征点的 特征描述子
//=======【3】载入点云=========================================
if (pcl::io::loadPCDFile (model_filename_, *model) < 0)
{
std::cout << "Error loading model cloud." << std::endl;
showHelp (argv[0]);
return (-1);
}
if (pcl::io::loadPCDFile (scene_filename_, *scene) < 0)
{
std::cout << "Error loading scene cloud." << std::endl;
showHelp (argv[0]);
return (-1);
}
//========【5】计算 法向量和曲率 ===========================================
pcl::NormalEstimationOMP<PointType, NormalType> norm_est;//多核 计算法线模型
norm_est.setKSearch (10);//最近10个点 协方差矩阵PCA分解 计算 法线向量
norm_est.setInputCloud (model);//模型点云
norm_est.compute (*model_normals);//模型点云的法线向量
norm_est.setInputCloud (scene);//场景点云
norm_est.compute (*scene_normals);//场景点云的法线向量
//=======【6】下采样滤波使用均匀采样（可以试试体素格子下采样）得到关键点==========
pcl::UniformSampling<PointType> uniform_sampling;//下采样滤波模型
uniform_sampling.setInputCloud (model);//模型点云
uniform_sampling.setRadiusSearch (model_ss_);//模型点云下采样滤波搜索半径
uniform_sampling.filter (*model_keypoints);//下采样得到的关键点
std::cout << "Model total points: " << model->size () << "; Selected Keypoints: "
<< model_keypoints->size () << std::endl;
uniform_sampling.setInputCloud (scene);//场景点云
uniform_sampling.setRadiusSearch (scene_ss_);//场景点云下采样滤波搜索半径
uniform_sampling.filter (*scene_keypoints);//下采样得到的关键点
std::cout << "Scene total points: " << scene->size () << "; Selected Keypoints: "
<< scene_keypoints->size () << std::endl;
//========【7】为keypoints关键点计算SHOT描述子Descriptor=================
pcl::SHOTEstimationOMP<PointType, NormalType, DescriptorType> descr_est;//shot描述子
descr_est.setRadiusSearch (descr_rad_);
descr_est.setInputCloud (model_keypoints);//输入模型的关键点
descr_est.setInputNormals (model_normals);//输入模型的法线
descr_est.setSearchSurface (model);//输入的点云
descr_est.compute (*model_descriptors);//模型点云描述子
descr_est.setInputCloud (scene_keypoints);
descr_est.setInputNormals (scene_normals);
descr_est.setSearchSurface (scene);
descr_est.compute (*scene_descriptors);//场景点云描述子
//========【8】按存储方法KDTree匹配两个点云（描述子向量匹配）点云分组得到匹配的组========
pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ());//最佳匹配点对组
pcl::KdTreeFLANN<DescriptorType> match_search;//匹配搜索 设置配准的方法
match_search.setInputCloud (model_descriptors);//模型点云描述子
std::vector<int> model_good_keypoints_indices;
std::vector<int> scene_good_keypoints_indices;
// 为 场景点云 的每一个关键点 匹配模型点云一个 描述子最相似的 点 < 0.25f
for (size_t i = 0; i < scene_descriptors->size (); ++i)
{
std::vector<int> neigh_indices (1);//设置最近邻点的索引
std::vector<float> neigh_sqr_dists (1);//申明最近邻平方距离值
if (!pcl_isfinite (scene_descriptors->at (i).descriptor[0]))//跳过NAN点
{
continue;
}
// sscene_descriptors->at (i)是给定点云描述子, 1是临近点个数 ，neigh_indices临近点的索引 neigh_sqr_dists是与临近点的平方距离值
int found_neighs = match_search.nearestKSearch (scene_descriptors->at (i), 1, neigh_indices, neigh_sqr_dists);
if (found_neighs == 1 && neigh_sqr_dists[0] < 0.25f)//（描述子与临近的距离在一般在0到1之间）才添加匹配
//在模型点云中 找 距离 场景点云点i shot描述子距离 <0.25 的点
{
pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
model_scene_corrs->push_back (corr);
model_good_keypoints_indices.push_back (corr.index_query);//模型点云好的 关键点
scene_good_keypoints_indices.push_back (corr.index_match);//场景点云好的管家的奶奶
}
}
pcl::PointCloud<PointType>::Ptr model_good_kp (new pcl::PointCloud<PointType> ());
pcl::PointCloud<PointType>::Ptr scene_good_kp (new pcl::PointCloud<PointType> ());
pcl::copyPointCloud (*model_keypoints, model_good_keypoints_indices, *model_good_kp);
pcl::copyPointCloud (*scene_keypoints, scene_good_keypoints_indices, *scene_good_kp);
std::cout << "Correspondences found: " << model_scene_corrs->size () << std::endl;
//===========【9】实际的配准方法的实现 执行聚类================
//变换矩阵 旋转矩阵与平移矩阵
// 对eigen中的固定大小的类使用STL容器的时候，如果直接使用就会出错 需要使用 Eigen::aligned_allocator 对齐技术
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
std::vector<pcl::Correspondences> clustered_corrs;//匹配点 相互连线的索引
// clustered_corrs[i][j].index_query 模型点 索引
// clustered_corrs[i][j].index_match 场景点 索引
if (use_hough_)// 使用 Hough3D 3D霍夫 算法寻找匹配点
{
//=========计算参考帧的Hough（也就是关键点）=========
pcl::PointCloud<RFType>::Ptr model_rf (new pcl::PointCloud<RFType> ());
pcl::PointCloud<RFType>::Ptr scene_rf (new pcl::PointCloud<RFType> ());
pcl::BOARDLocalReferenceFrameEstimation<PointType, NormalType, RFType> rf_est;
rf_est.setFindHoles (true);
rf_est.setRadiusSearch (rf_rad_);
rf_est.setInputCloud (model_keypoints);
rf_est.setInputNormals (model_normals);
rf_est.setSearchSurface (model);
rf_est.compute (*model_rf);
rf_est.setInputCloud (scene_keypoints);
rf_est.setInputNormals (scene_normals);
rf_est.setSearchSurface (scene);
rf_est.compute (*scene_rf);
// 聚类 聚类的方法 Clustering
//对输入点与的聚类，以区分不同的实例的场景中的模型
pcl::Hough3DGrouping<PointType, PointType, RFType, RFType> clusterer;
clusterer.setHoughBinSize (cg_size_);
clusterer.setHoughThreshold (cg_thresh_);
clusterer.setUseInterpolation (true);
clusterer.setUseDistanceWeight (false);
clusterer.setInputCloud (model_keypoints);
clusterer.setInputRf (model_rf);
clusterer.setSceneCloud (scene_keypoints);
clusterer.setSceneRf (scene_rf);
clusterer.setModelSceneCorrespondences (model_scene_corrs);
clusterer.recognize (rototranslations, clustered_corrs);
}
else// 或者使用几何一致性性质 Using GeometricConsistency
{
pcl::GeometricConsistencyGrouping<PointType, PointType> gc_clusterer;
gc_clusterer.setGCSize (cg_size_);//设置几何一致性的大小
gc_clusterer.setGCThreshold (cg_thresh_);//阀值
gc_clusterer.setInputCloud (model_keypoints);
gc_clusterer.setSceneCloud (scene_keypoints);
gc_clusterer.setModelSceneCorrespondences (model_scene_corrs);
gc_clusterer.recognize (rototranslations, clustered_corrs);
}
/**
* Stop if no instances
*/
if (rototranslations.size () <= 0)
{
cout << "*** No instances found! ***" << endl;
return (0);
}
else
{
cout << "Recognized Instances: " << rototranslations.size () << endl << endl;
}
/**
* Generates clouds for each instances found
*/
std::vector<pcl::PointCloud<PointType>::ConstPtr> instances;
for (size_t i = 0; i < rototranslations.size (); ++i)
{
pcl::PointCloud<PointType>::Ptr rotated_model (new pcl::PointCloud<PointType> ());
pcl::transformPointCloud (*model, *rotated_model, rototranslations[i]);
instances.push_back (rotated_model);
}
//ICP 点云配准==========================================================
std::vector<pcl::PointCloud<PointType>::ConstPtr> registered_instances;
if (true)
{
cout << "--- ICP ---------" << endl;
for (size_t i = 0; i < rototranslations.size (); ++i)
{
pcl::IterativeClosestPoint<PointType, PointType> icp;
icp.setMaximumIterations (icp_max_iter_);
icp.setMaxCorrespondenceDistance (icp_corr_distance_);
icp.setInputTarget (scene);//场景
icp.setInputSource (instances[i]);//场景中的实例
pcl::PointCloud<PointType>::Ptr registered (new pcl::PointCloud<PointType>);
icp.align (*registered);//匹配到的点云
registered_instances.push_back (registered);
cout << "Instance " << i << " ";
if (icp.hasConverged ())
{
cout << "Aligned!" << endl;
}
else
{
cout << "Not Aligned!" << endl;
}
}
cout << "-----------------" << endl << endl;
}
// 模型假设验证 Hypothesis Verification=====================================
cout << "--- Hypotheses Verification ---" << endl;
std::vector<bool> hypotheses_mask; // Mask Vector to identify positive hypotheses
pcl::GlobalHypothesesVerification<PointType, PointType> GoHv;
GoHv.setSceneCloud (scene); // Scene Cloud
GoHv.addModels (registered_instances, true); //Models to verify
GoHv.setInlierThreshold (hv_inlier_th_);
GoHv.setOcclusionThreshold (hv_occlusion_th_);
GoHv.setRegularizer (hv_regularizer_);
GoHv.setRadiusClutter (hv_rad_clutter_);
GoHv.setClutterRegularizer (hv_clutter_reg_);
GoHv.setDetectClutter (hv_detect_clutter_);
GoHv.setRadiusNormals (hv_rad_normals_);
GoHv.verify ();
GoHv.getMask (hypotheses_mask); // i-element TRUE if hvModels[i] verifies hypotheses
for (int i = 0; i < hypotheses_mask.size (); i++)
{
if (hypotheses_mask[i])
{
cout << "Instance " << i << " is GOOD! <---" << endl;
}
else
{
cout << "Instance " << i << " is bad!" << endl;
}
}
cout << "-------------------------------" << endl;
//======可视化 Visualization====================================================
pcl::visualization::PCLVisualizer viewer ("Hypotheses Verification");
viewer.addPointCloud (scene, "scene_cloud");
pcl::PointCloud<PointType>::Ptr off_scene_model (new pcl::PointCloud<PointType> ());
pcl::PointCloud<PointType>::Ptr off_scene_model_keypoints (new pcl::PointCloud<PointType> ());
pcl::PointCloud<PointType>::Ptr off_model_good_kp (new pcl::PointCloud<PointType> ());
pcl::transformPointCloud (*model, *off_scene_model, Eigen::Vector3f (-1, 0, 0), Eigen::Quaternionf (1, 0, 0, 0));
pcl::transformPointCloud (*model_keypoints, *off_scene_model_keypoints, Eigen::Vector3f (-1, 0, 0), Eigen::Quaternionf (1, 0, 0, 0));
pcl::transformPointCloud (*model_good_kp, *off_model_good_kp, Eigen::Vector3f (-1, 0, 0), Eigen::Quaternionf (1, 0, 0, 0));
if (show_keypoints_)
{
CloudStyle modelStyle = style_white;
pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_color_handler (off_scene_model, modelStyle.r, modelStyle.g, modelStyle.b);
viewer.addPointCloud (off_scene_model, off_scene_model_color_handler, "off_scene_model");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, modelStyle.size, "off_scene_model");
}
if (show_keypoints_)
{
CloudStyle goodKeypointStyle = style_violet;
pcl::visualization::PointCloudColorHandlerCustom<PointType> model_good_keypoints_color_handler (off_model_good_kp, goodKeypointStyle.r, goodKeypointStyle.g,
goodKeypointStyle.b);
viewer.addPointCloud (off_model_good_kp, model_good_keypoints_color_handler, "model_good_keypoints");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, goodKeypointStyle.size, "model_good_keypoints");
pcl::visualization::PointCloudColorHandlerCustom<PointType> scene_good_keypoints_color_handler (scene_good_kp, goodKeypointStyle.r, goodKeypointStyle.g,
goodKeypointStyle.b);
viewer.addPointCloud (scene_good_kp, scene_good_keypoints_color_handler, "scene_good_keypoints");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, goodKeypointStyle.size, "scene_good_keypoints");
}
for (size_t i = 0; i < instances.size (); ++i)
{
std::stringstream ss_instance;
ss_instance << "instance_" << i;
CloudStyle clusterStyle = style_red;
pcl::visualization::PointCloudColorHandlerCustom<PointType> instance_color_handler (instances[i], clusterStyle.r, clusterStyle.g, clusterStyle.b);
viewer.addPointCloud (instances[i], instance_color_handler, ss_instance.str ());
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, clusterStyle.size, ss_instance.str ());
CloudStyle registeredStyles = hypotheses_mask[i] ? style_green : style_cyan;
ss_instance << "_registered" << endl;
pcl::visualization::PointCloudColorHandlerCustom<PointType> registered_instance_color_handler (registered_instances[i], registeredStyles.r,
registeredStyles.g, registeredStyles.b);
viewer.addPointCloud (registered_instances[i], registered_instance_color_handler, ss_instance.str ());
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, registeredStyles.size, ss_instance.str ());
}
while (!viewer.wasStopped ())
{
viewer.spinOnce ();
}
return (0);
}
int
main (int argc,
char *argv[])
{
parseCommandLine (argc, argv);
pcl::PointCloud<PointType>::Ptr model (new pcl::PointCloud<PointType> ());
pcl::PointCloud<PointType>::Ptr model_keypoints (new pcl::PointCloud<PointType> ());
pcl::PointCloud<PointType>::Ptr scene (new pcl::PointCloud<PointType> ());
pcl::PointCloud<PointType>::Ptr scene_keypoints (new pcl::PointCloud<PointType> ());
pcl::PointCloud<NormalType>::Ptr model_normals (new pcl::PointCloud<NormalType> ());
pcl::PointCloud<NormalType>::Ptr scene_normals (new pcl::PointCloud<NormalType> ());
pcl::PointCloud<DescriptorType>::Ptr model_descriptors (new pcl::PointCloud<DescriptorType> ());
pcl::PointCloud<DescriptorType>::Ptr scene_descriptors (new pcl::PointCloud<DescriptorType> ());
/**
* Load Clouds
*/
if (pcl::io::loadPCDFile (model_filename_, *model) < 0)
{
std::cout << "Error loading model cloud." << std::endl;
showHelp (argv[0]);
return (-1);
}
if (pcl::io::loadPCDFile (scene_filename_, *scene) < 0)
{
std::cout << "Error loading scene cloud." << std::endl;
showHelp (argv[0]);
return (-1);
}
/**
* Compute Normals
*/
pcl::NormalEstimationOMP<PointType, NormalType> norm_est;
norm_est.setKSearch (10);
norm_est.setInputCloud (model);
norm_est.compute (*model_normals);
norm_est.setInputCloud (scene);
norm_est.compute (*scene_normals);
/**
* Downsample Clouds to Extract keypoints
*/
pcl::UniformSampling<PointType> uniform_sampling;
uniform_sampling.setInputCloud (model);
uniform_sampling.setRadiusSearch (model_ss_);
uniform_sampling.filter (*model_keypoints);
std::cout << "Model total points: " << model->size () << "; Selected Keypoints: " << model_keypoints->size () << std::endl;
uniform_sampling.setInputCloud (scene);
uniform_sampling.setRadiusSearch (scene_ss_);
uniform_sampling.filter (*scene_keypoints);
std::cout << "Scene total points: " << scene->size () << "; Selected Keypoints: " << scene_keypoints->size () << std::endl;
/**
* Compute Descriptor for keypoints
*/
pcl::SHOTEstimationOMP<PointType, NormalType, DescriptorType> descr_est;
descr_est.setRadiusSearch (descr_rad_);
descr_est.setInputCloud (model_keypoints);
descr_est.setInputNormals (model_normals);
descr_est.setSearchSurface (model);
descr_est.compute (*model_descriptors);
descr_est.setInputCloud (scene_keypoints);
descr_est.setInputNormals (scene_normals);
descr_est.setSearchSurface (scene);
descr_est.compute (*scene_descriptors);
/**
* Find Model-Scene Correspondences with KdTree
*/
pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ());
pcl::KdTreeFLANN<DescriptorType> match_search;
match_search.setInputCloud (model_descriptors);
std::vector<int> model_good_keypoints_indices;
std::vector<int> scene_good_keypoints_indices;
for (size_t i = 0; i < scene_descriptors->size (); ++i)
{
std::vector<int> neigh_indices (1);
std::vector<float> neigh_sqr_dists (1);
if (!pcl_isfinite (scene_descriptors->at (i).descriptor[0])) //skipping NaNs
{
continue;
}
int found_neighs = match_search.nearestKSearch (scene_descriptors->at (i), 1, neigh_indices, neigh_sqr_dists);
if (found_neighs == 1 && neigh_sqr_dists[0] < 0.25f)
{
pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
model_scene_corrs->push_back (corr);
model_good_keypoints_indices.push_back (corr.index_query);
scene_good_keypoints_indices.push_back (corr.index_match);
}
}
pcl::PointCloud<PointType>::Ptr model_good_kp (new pcl::PointCloud<PointType> ());
pcl::PointCloud<PointType>::Ptr scene_good_kp (new pcl::PointCloud<PointType> ());
pcl::copyPointCloud (*model_keypoints, model_good_keypoints_indices, *model_good_kp);
pcl::copyPointCloud (*scene_keypoints, scene_good_keypoints_indices, *scene_good_kp);
std::cout << "Correspondences found: " << model_scene_corrs->size () << std::endl;
/**
* Clustering
*/
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
std::vector < pcl::Correspondences > clustered_corrs;
if (use_hough_)
{
pcl::PointCloud<RFType>::Ptr model_rf (new pcl::PointCloud<RFType> ());
pcl::PointCloud<RFType>::Ptr scene_rf (new pcl::PointCloud<RFType> ());
pcl::BOARDLocalReferenceFrameEstimation<PointType, NormalType, RFType> rf_est;
rf_est.setFindHoles (true);
rf_est.setRadiusSearch (rf_rad_);
rf_est.setInputCloud (model_keypoints);
rf_est.setInputNormals (model_normals);
rf_est.setSearchSurface (model);
rf_est.compute (*model_rf);
rf_est.setInputCloud (scene_keypoints);
rf_est.setInputNormals (scene_normals);
rf_est.setSearchSurface (scene);
rf_est.compute (*scene_rf);
// Clustering
pcl::Hough3DGrouping<PointType, PointType, RFType, RFType> clusterer;
clusterer.setHoughBinSize (cg_size_);
clusterer.setHoughThreshold (cg_thresh_);
clusterer.setUseInterpolation (true);
clusterer.setUseDistanceWeight (false);
clusterer.setInputCloud (model_keypoints);
clusterer.setInputRf (model_rf);
clusterer.setSceneCloud (scene_keypoints);
clusterer.setSceneRf (scene_rf);
clusterer.setModelSceneCorrespondences (model_scene_corrs);
clusterer.recognize (rototranslations, clustered_corrs);
}
else
{
pcl::GeometricConsistencyGrouping<PointType, PointType> gc_clusterer;
gc_clusterer.setGCSize (cg_size_);
gc_clusterer.setGCThreshold (cg_thresh_);
gc_clusterer.setInputCloud (model_keypoints);
gc_clusterer.setSceneCloud (scene_keypoints);
gc_clusterer.setModelSceneCorrespondences (model_scene_corrs);
gc_clusterer.recognize (rototranslations, clustered_corrs);
}
/**
* Stop if no instances
*/
if (rototranslations.size () <= 0)
{
cout << "*** No instances found! ***" << endl;
return (0);
}
else
{
cout << "Recognized Instances: " << rototranslations.size () << endl << endl;
}
/**
* Generates clouds for each instances found
*/
std::vector<pcl::PointCloud<PointType>::ConstPtr> instances;
for (size_t i = 0; i < rototranslations.size (); ++i)
{
pcl::PointCloud<PointType>::Ptr rotated_model (new pcl::PointCloud<PointType> ());
pcl::transformPointCloud (*model, *rotated_model, rototranslations[i]);
instances.push_back (rotated_model);
}
/**
* ICP
*/
std::vector<pcl::PointCloud<PointType>::ConstPtr> registered_instances;
if (true)
{
cout << "--- ICP ---------" << endl;
for (size_t i = 0; i < rototranslations.size (); ++i)
{
pcl::IterativeClosestPoint<PointType, PointType> icp;
icp.setMaximumIterations (icp_max_iter_);
icp.setMaxCorrespondenceDistance (icp_corr_distance_);
icp.setInputTarget (scene);
icp.setInputSource (instances[i]);
pcl::PointCloud<PointType>::Ptr registered (new pcl::PointCloud<PointType>);
icp.align (*registered);
registered_instances.push_back (registered);
cout << "Instance " << i << " ";
if (icp.hasConverged ())
{
cout << "Aligned!" << endl;
}
else
{
cout << "Not Aligned!" << endl;
}
}
cout << "-----------------" << endl << endl;
}
/**
* Hypothesis Verification
*/
cout << "--- Hypotheses Verification ---" << endl;
std::vector<bool> hypotheses_mask; // Mask Vector to identify positive hypotheses
pcl::GlobalHypothesesVerification<PointType, PointType> GoHv;
GoHv.setSceneCloud (scene); // Scene Cloud
GoHv.addModels (registered_instances, true); //Models to verify
GoHv.setInlierThreshold (hv_inlier_th_);
GoHv.setOcclusionThreshold (hv_occlusion_th_);
GoHv.setRegularizer (hv_regularizer_);
GoHv.setRadiusClutter (hv_rad_clutter_);
GoHv.setClutterRegularizer (hv_clutter_reg_);
GoHv.setDetectClutter (hv_detect_clutter_);
GoHv.setRadiusNormals (hv_rad_normals_);
GoHv.verify ();
GoHv.getMask (hypotheses_mask); // i-element TRUE if hvModels[i] verifies hypotheses
for (int i = 0; i < hypotheses_mask.size (); i++)
{
if (hypotheses_mask[i])
{
cout << "Instance " << i << " is GOOD! <---" << endl;
}
else
{
cout << "Instance " << i << " is bad!" << endl;
}
}
cout << "-------------------------------" << endl;
/**
* Visualization
*/
pcl::visualization::PCLVisualizer viewer ("Hypotheses Verification");
viewer.addPointCloud (scene, "scene_cloud");
pcl::PointCloud<PointType>::Ptr off_scene_model (new pcl::PointCloud<PointType> ());
pcl::PointCloud<PointType>::Ptr off_scene_model_keypoints (new pcl::PointCloud<PointType> ());
pcl::PointCloud<PointType>::Ptr off_model_good_kp (new pcl::PointCloud<PointType> ());
pcl::transformPointCloud (*model, *off_scene_model, Eigen::Vector3f (-1, 0, 0), Eigen::Quaternionf (1, 0, 0, 0));
pcl::transformPointCloud (*model_keypoints, *off_scene_model_keypoints, Eigen::Vector3f (-1, 0, 0), Eigen::Quaternionf (1, 0, 0, 0));
pcl::transformPointCloud (*model_good_kp, *off_model_good_kp, Eigen::Vector3f (-1, 0, 0), Eigen::Quaternionf (1, 0, 0, 0));
if (show_keypoints_)
{
CloudStyle modelStyle = style_white;
pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_color_handler (off_scene_model, modelStyle.r, modelStyle.g, modelStyle.b);
viewer.addPointCloud (off_scene_model, off_scene_model_color_handler, "off_scene_model");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, modelStyle.size, "off_scene_model");
}
if (show_keypoints_)
{
CloudStyle goodKeypointStyle = style_violet;
pcl::visualization::PointCloudColorHandlerCustom<PointType> model_good_keypoints_color_handler (off_model_good_kp, goodKeypointStyle.r, goodKeypointStyle.g,
goodKeypointStyle.b);
viewer.addPointCloud (off_model_good_kp, model_good_keypoints_color_handler, "model_good_keypoints");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, goodKeypointStyle.size, "model_good_keypoints");
pcl::visualization::PointCloudColorHandlerCustom<PointType> scene_good_keypoints_color_handler (scene_good_kp, goodKeypointStyle.r, goodKeypointStyle.g,
goodKeypointStyle.b);
viewer.addPointCloud (scene_good_kp, scene_good_keypoints_color_handler, "scene_good_keypoints");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, goodKeypointStyle.size, "scene_good_keypoints");
}
for (size_t i = 0; i < instances.size (); ++i)
{
std::stringstream ss_instance;
ss_instance << "instance_" << i;
CloudStyle clusterStyle = style_red;
pcl::visualization::PointCloudColorHandlerCustom<PointType> instance_color_handler (instances[i], clusterStyle.r, clusterStyle.g, clusterStyle.b);
viewer.addPointCloud (instances[i], instance_color_handler, ss_instance.str ());
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, clusterStyle.size, ss_instance.str ());
CloudStyle registeredStyles = hypotheses_mask[i] ? style_green : style_cyan;
ss_instance << "_registered" << endl;
pcl::visualization::PointCloudColorHandlerCustom<PointType> registered_instance_color_handler (registered_instances[i], registeredStyles.r,
registeredStyles.g, registeredStyles.b);
viewer.addPointCloud (registered_instances[i], registered_instance_color_handler, ss_instance.str ());
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, registeredStyles.size, ss_instance.str ());
}
while (!viewer.wasStopped ())
{
viewer.spinOnce ();
}
return (0);
}
int
main (int argc, char *argv[])
{
parseCommandLine (argc, argv);
pcl::PointCloud<PointType>::Ptr model (new pcl::PointCloud<PointType> ());
pcl::PointCloud<PointType>::Ptr model_keypoints (new pcl::PointCloud<PointType> ());
pcl::PointCloud<PointType>::Ptr scene (new pcl::PointCloud<PointType> ());
pcl::PointCloud<PointType>::Ptr scene_keypoints (new pcl::PointCloud<PointType> ());
pcl::PointCloud<NormalType>::Ptr model_normals (new pcl::PointCloud<NormalType> ());
pcl::PointCloud<NormalType>::Ptr scene_normals (new pcl::PointCloud<NormalType> ());
pcl::PointCloud<DescriptorType>::Ptr model_descriptors (new pcl::PointCloud<DescriptorType> ());
pcl::PointCloud<DescriptorType>::Ptr scene_descriptors (new pcl::PointCloud<DescriptorType> ());
//
// Load clouds
//
if (pcl::io::loadPCDFile (model_filename_, *model) < 0)
{
std::cout << "Error loading model cloud." << std::endl;
showHelp (argv[0]);
return (-1);
}
if (pcl::io::loadPCDFile (scene_filename_, *scene) < 0)
{
std::cout << "Error loading scene cloud." << std::endl;
showHelp (argv[0]);
return (-1);
}
//
// Set up resolution invariance
//
if (use_cloud_resolution_)
{
float resolution = static_cast<float> (computeCloudResolution (model));
if (resolution != 0.0f)
{
model_ss_ *= resolution;
scene_ss_ *= resolution;
rf_rad_ *= resolution;
descr_rad_ *= resolution;
cg_size_ *= resolution;
}
std::cout << "Model resolution: " << resolution << std::endl;
std::cout << "Model sampling size: " << model_ss_ << std::endl;
std::cout << "Scene sampling size: " << scene_ss_ << std::endl;
std::cout << "LRF support radius: " << rf_rad_ << std::endl;
std::cout << "SHOT descriptor radius: " << descr_rad_ << std::endl;
std::cout << "Clustering bin size: " << cg_size_ << std::endl << std::endl;
}
//
// Compute Normals
//
pcl::NormalEstimationOMP<PointType, NormalType> norm_est;
norm_est.setKSearch (10);
norm_est.setInputCloud (model);
norm_est.compute (*model_normals);
norm_est.setInputCloud (scene);
norm_est.compute (*scene_normals);
//
// Downsample Clouds to Extract keypoints
//
/*
pcl::UniformSampling<PointType> uniform_sampling;
uniform_sampling.setInputCloud (model);
uniform_sampling.setRadiusSearch (model_ss_);
uniform_sampling.filter (*model_keypoints);
std::cout << "Model total points: " << model->size () << "; Selected Keypoints: " << model_keypoints->size () << std::endl;
uniform_sampling.setInputCloud (scene);
uniform_sampling.setRadiusSearch (scene_ss_);
uniform_sampling.filter (*scene_keypoints);
std::cout << "Scene total points: " << scene->size () << "; Selected Keypoints: " << scene_keypoints->size () << std::endl;
*/
pcl::UniformSampling<PointType> uniform_sampling;
uniform_sampling.setInputCloud (model);
uniform_sampling.setRadiusSearch (model_ss_);
//uniform_sampling.filter (*model_keypoints);
pcl::PointCloud<int> keypointIndices1;
uniform_sampling.compute(keypointIndices1);
pcl::copyPointCloud(*model, keypointIndices1.points, *model_keypoints);
std::cout << "Model total points: " << model->size () << "; Selected Keypoints: " << model_keypoints->size () << std::endl;
uniform_sampling.setInputCloud (scene);
uniform_sampling.setRadiusSearch (scene_ss_);
//uniform_sampling.filter (*scene_keypoints);
pcl::PointCloud<int> keypointIndices2;
uniform_sampling.compute(keypointIndices2);
pcl::copyPointCloud(*scene, keypointIndices2.points, *scene_keypoints);
std::cout << "Scene total points: " << scene->size () << "; Selected Keypoints: " << scene_keypoints->size () << std::endl;
//
// Compute Descriptor for keypoints
//
pcl::SHOTEstimationOMP<PointType, NormalType, DescriptorType> descr_est;
descr_est.setRadiusSearch (descr_rad_);
descr_est.setInputCloud (model_keypoints);
descr_est.setInputNormals (model_normals);
descr_est.setSearchSurface (model);
descr_est.compute (*model_descriptors);
descr_est.setInputCloud (scene_keypoints);
descr_est.setInputNormals (scene_normals);
descr_est.setSearchSurface (scene);
descr_est.compute (*scene_descriptors);
//
// Find Model-Scene Correspondences with KdTree
//
pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ());
pcl::KdTreeFLANN<DescriptorType> match_search;
match_search.setInputCloud (model_descriptors);
// For each scene keypoint descriptor, find nearest neighbor into the model keypoints descriptor cloud and add it to the correspondences vector.
for (size_t i = 0; i < scene_descriptors->size (); ++i)
{
std::vector<int> neigh_indices (1);
std::vector<float> neigh_sqr_dists (1);
if (!pcl_isfinite (scene_descriptors->at (i).descriptor[0])) //skipping NaNs
{
continue;
}
int found_neighs = match_search.nearestKSearch (scene_descriptors->at (i), 1, neigh_indices, neigh_sqr_dists);
if(found_neighs == 1 && neigh_sqr_dists[0] < 0.25f) // add match only if the squared descriptor distance is less than 0.25 (SHOT descriptor distances are between 0 and 1 by design)
{
pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
model_scene_corrs->push_back (corr);
}
}
std::cout << "Correspondences found: " << model_scene_corrs->size () << std::endl;
//
// Actual Clustering
//
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
std::vector<pcl::Correspondences> clustered_corrs;
// Using Hough3D
if (use_hough_)
{
//
// Compute (Keypoints) Reference Frames only for Hough
//
pcl::PointCloud<RFType>::Ptr model_rf (new pcl::PointCloud<RFType> ());
pcl::PointCloud<RFType>::Ptr scene_rf (new pcl::PointCloud<RFType> ());
pcl::BOARDLocalReferenceFrameEstimation<PointType, NormalType, RFType> rf_est;
rf_est.setFindHoles (true);
rf_est.setRadiusSearch (rf_rad_);
rf_est.setInputCloud (model_keypoints);
rf_est.setInputNormals (model_normals);
rf_est.setSearchSurface (model);
rf_est.compute (*model_rf);
rf_est.setInputCloud (scene_keypoints);
rf_est.setInputNormals (scene_normals);
rf_est.setSearchSurface (scene);
rf_est.compute (*scene_rf);
// Clustering
pcl::Hough3DGrouping<PointType, PointType, RFType, RFType> clusterer;
clusterer.setHoughBinSize (cg_size_);
clusterer.setHoughThreshold (cg_thresh_);
clusterer.setUseInterpolation (true);
clusterer.setUseDistanceWeight (false);
clusterer.setInputCloud (model_keypoints);
clusterer.setInputRf (model_rf);
clusterer.setSceneCloud (scene_keypoints);
clusterer.setSceneRf (scene_rf);
clusterer.setModelSceneCorrespondences (model_scene_corrs);
//clusterer.cluster (clustered_corrs);
clusterer.recognize (rototranslations, clustered_corrs);
}
else // Using GeometricConsistency
{
pcl::GeometricConsistencyGrouping<PointType, PointType> gc_clusterer;
gc_clusterer.setGCSize (cg_size_);
gc_clusterer.setGCThreshold (cg_thresh_);
gc_clusterer.setInputCloud (model_keypoints);
gc_clusterer.setSceneCloud (scene_keypoints);
gc_clusterer.setModelSceneCorrespondences (model_scene_corrs);
//gc_clusterer.cluster (clustered_corrs);
gc_clusterer.recognize (rototranslations, clustered_corrs);
}
//
// Output results
//
std::cout << "Model instances found: " << rototranslations.size () << std::endl;
for (size_t i = 0; i < rototranslations.size (); ++i)
{
std::cout << "\n Instance " << i + 1 << ":" << std::endl;
std::cout << " Correspondences belonging to this instance: " << clustered_corrs[i].size () << std::endl;
// Print the rotation matrix and translation vector
Eigen::Matrix3f rotation = rototranslations[i].block<3,3>(0, 0);
Eigen::Vector3f translation = rototranslations[i].block<3,1>(0, 3);
printf ("\n");
printf (" | %6.3f %6.3f %6.3f | \n", rotation (0,0), rotation (0,1), rotation (0,2));
printf (" R = | %6.3f %6.3f %6.3f | \n", rotation (1,0), rotation (1,1), rotation (1,2));
printf (" | %6.3f %6.3f %6.3f | \n", rotation (2,0), rotation (2,1), rotation (2,2));
printf ("\n");
printf (" t = < %0.3f, %0.3f, %0.3f >\n", translation (0), translation (1), translation (2));
}
//
// Visualization
//
pcl::visualization::PCLVisualizer viewer ("Correspondence Grouping");
viewer.addPointCloud (scene, "scene_cloud");
pcl::PointCloud<PointType>::Ptr off_scene_model (new pcl::PointCloud<PointType> ());
pcl::PointCloud<PointType>::Ptr off_scene_model_keypoints (new pcl::PointCloud<PointType> ());
if (show_correspondences_ || show_keypoints_)
{
// We are translating the model so that it doesn't end in the middle of the scene representation
pcl::transformPointCloud (*model, *off_scene_model, Eigen::Vector3f (-1,0,0), Eigen::Quaternionf (1, 0, 0, 0));
pcl::transformPointCloud (*model_keypoints, *off_scene_model_keypoints, Eigen::Vector3f (-1,0,0), Eigen::Quaternionf (1, 0, 0, 0));
pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_color_handler (off_scene_model, 255, 255, 128);
viewer.addPointCloud (off_scene_model, off_scene_model_color_handler, "off_scene_model");
}
if (show_keypoints_)
{
pcl::visualization::PointCloudColorHandlerCustom<PointType> scene_keypoints_color_handler (scene_keypoints, 0, 0, 255);
viewer.addPointCloud (scene_keypoints, scene_keypoints_color_handler, "scene_keypoints");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "scene_keypoints");
pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_keypoints_color_handler (off_scene_model_keypoints, 0, 0, 255);
viewer.addPointCloud (off_scene_model_keypoints, off_scene_model_keypoints_color_handler, "off_scene_model_keypoints");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "off_scene_model_keypoints");
}
for (size_t i = 0; i < rototranslations.size (); ++i)
{
pcl::PointCloud<PointType>::Ptr rotated_model (new pcl::PointCloud<PointType> ());
pcl::transformPointCloud (*model, *rotated_model, rototranslations[i]);
std::stringstream ss_cloud;
ss_cloud << "instance" << i;
pcl::visualization::PointCloudColorHandlerCustom<PointType> rotated_model_color_handler (rotated_model, 255, 0, 0);
viewer.addPointCloud (rotated_model, rotated_model_color_handler, ss_cloud.str ());
if (show_correspondences_)
{
for (size_t j = 0; j < clustered_corrs[i].size (); ++j)
{
std::stringstream ss_line;
ss_line << "correspondence_line" << i << "_" << j;
PointType& model_point = off_scene_model_keypoints->at (clustered_corrs[i][j].index_query);
PointType& scene_point = scene_keypoints->at (clustered_corrs[i][j].index_match);
// We are drawing a line for each pair of clustered correspondences found between the model and the scene
viewer.addLine<PointType, PointType> (model_point, scene_point, 0, 255, 0, ss_line.str ());
}
}
}
while (!viewer.wasStopped ())
{
viewer.spinOnce ();
}
return (0);
}
Eigen::Matrix4f DenseColorICP(const pcl::PointCloud<PointT>::Ptr source, const pcl::PointCloud<PointT>::Ptr target, Eigen::Matrix4f &initial_guess)
{
//if the ICP fail to converge, return false. Otherwise, return true!
size_t Iter_num = 50;
float icp_inlier_threshold = 0.01;
pcl::PointCloud<PointT>::Ptr init_source(new pcl::PointCloud<PointT>);
pcl::transformPointCloud(*source, *init_source, initial_guess);
pcl::PointCloud<pcl::PointXYZHSV>::Ptr source_hue(new pcl::PointCloud<pcl::PointXYZHSV>);
pcl::PointCloud<pcl::PointXYZHSV>::Ptr target_hue(new pcl::PointCloud<pcl::PointXYZHSV>);
ExtractHue(init_source, source_hue);
ExtractHue(target, target_hue);
Eigen::Matrix4f final_tran = initial_guess;
pcl::search::KdTree<pcl::PointXYZHSV> target_tree;
target_tree.setInputCloud(target_hue);
pcl::registration::TransformationEstimationSVD<pcl::PointXYZHSV, pcl::PointXYZHSV> SVD;
double last_error = 100000;
for( size_t iter = 0 ; iter < Iter_num ; iter++ )
{
//In each round of ICP, transform the model_hue cloud
float diffh, diffs, diffv, diffsum;
pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ());
for( size_t i = 0 ; i < source_hue->size() ; i++ )
{
std::vector<int> pointIdx;
std::vector<float> pointDistance;
if ( pcl_isfinite(source_hue->at(i).z) && target_tree.radiusSearch (source_hue->at(i), 0.01, pointIdx, pointDistance) > 0 )
{
float diffmin = 1000;
int diffmin_idx = -1;
for (size_t j = 0; j < pointIdx.size (); j++)
{
//std::cerr<<"Finding..."<<scene_hue->points[pointIdx[j]].h <<" "<<model_hue->points[i].h<<std::endl;
if( pointDistance.at(j) < icp_inlier_threshold )
{
diffh = std::min( fabs(target_hue->points[pointIdx[j]].h - source_hue->points[i].h),
std::min(fabs(target_hue->points[pointIdx[j]].h - 1 - source_hue->points[i].h), fabs(target_hue->points[pointIdx[j]].h + 1 - source_hue->points[i].h)));
diffs = fabs(target_hue->points[pointIdx[j]].s - source_hue->points[i].s);
diffv = fabs(target_hue->points[pointIdx[j]].v - source_hue->points[i].v);
diffsum = 2*diffh + 2*diffs + diffv;
if( diffmin > diffsum )
{
diffmin = diffsum;
diffmin_idx = j;
}
}
}
//std::cerr<<diffcurvature<<" ";
//std::cerr<<diffmin<<" ";
if( diffmin <= 0.1 )
{
pcl::Correspondence temp;
temp.index_query = i;
temp.index_match = pointIdx[diffmin_idx];
temp.distance = pointDistance.at(diffmin_idx);
model_scene_corrs->push_back(temp);
}
}
}
Eigen::Matrix4f svdRt;
SVD.estimateRigidTransformation(*source_hue, *target_hue, *model_scene_corrs, svdRt);
pcl::transformPointCloud(*source_hue, *source_hue, svdRt);
final_tran = svdRt * final_tran ;
std::cerr<<"Ratio "<<(model_scene_corrs->size()+0.0) / source_hue->size()<<std::endl;
if( (model_scene_corrs->size()+0.0) / source_hue->size() >= 0.2 ) //sufficient inlier found
{
size_t corrs = model_scene_corrs->size();
double this_error=0;
for( size_t j = 0; j < corrs; j++ )
{
pcl::PointXYZHSV source_pt = source_hue->points[model_scene_corrs->at(j).index_query];
pcl::PointXYZHSV target_pt = target_hue->points[model_scene_corrs->at(j).index_match];
double diffx = source_pt.x - target_pt.x, diffy = source_pt.y - target_pt.y, diffz = source_pt.z - target_pt.z;
double dist = sqrt( diffx*diffx + diffy*diffy + diffz*diffz );
this_error += dist;
}
this_error = this_error / corrs;
if( fabs(this_error - last_error) < 1e-6 ) //Convergence reach
{
std::cerr<<"Convergence Reached. Error: "<<this_error<<std::endl;
std::cerr<<"Iter Num: "<<iter<<std::endl;
return final_tran;
}
else
last_error = this_error;
}
}
return final_tran;
}
void GeometricConsistencyGrouping::recognize(
const sensor_msgs::PointCloud2::ConstPtr& scene_cloud_msg,
const sensor_msgs::PointCloud2::ConstPtr& scene_feature_msg)
{
boost::mutex::scoped_lock lock(mutex_);
if (!reference_cloud_ || !reference_feature_) {
NODELET_ERROR_THROTTLE(1.0, "Not yet reference is available");
return;
}
pcl::PointCloud<pcl::SHOT352>::Ptr
scene_feature (new pcl::PointCloud<pcl::SHOT352>);
pcl::PointCloud<pcl::PointNormal>::Ptr
scene_cloud (new pcl::PointCloud<pcl::PointNormal>);
pcl::fromROSMsg(*scene_cloud_msg, *scene_cloud);
pcl::fromROSMsg(*scene_feature_msg, *scene_feature);
pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ());
pcl::KdTreeFLANN<pcl::SHOT352> match_search;
match_search.setInputCloud (reference_feature_);
for (size_t i = 0; i < scene_feature->size(); ++i)
{
std::vector<int> neigh_indices(1);
std::vector<float> neigh_sqr_dists(1);
if (!pcl_isfinite (scene_feature->at(i).descriptor[0])) { //skipping NaNs
continue;
}
int found_neighs
= match_search.nearestKSearch(scene_feature->at(i), 1,
neigh_indices, neigh_sqr_dists);
// add match only if the squared descriptor distance is less than 0.25
// (SHOT descriptor distances are between 0 and 1 by design)
if (found_neighs == 1 && neigh_sqr_dists[0] < 0.25f) {
pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
model_scene_corrs->push_back (corr);
}
}
pcl::GeometricConsistencyGrouping<pcl::PointNormal, pcl::PointNormal> gc_clusterer;
gc_clusterer.setGCSize(gc_size_);
gc_clusterer.setGCThreshold(gc_thresh_);
gc_clusterer.setInputCloud(reference_cloud_);
gc_clusterer.setSceneCloud(scene_cloud);
gc_clusterer.setModelSceneCorrespondences(model_scene_corrs);
//gc_clusterer.cluster (clustered_corrs);
std::vector<pcl::Correspondences> clustered_corrs;
std::vector<Eigen::Matrix4f,
Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
gc_clusterer.recognize(rototranslations, clustered_corrs);
if (rototranslations.size() > 0) {
NODELET_INFO("detected %lu objects", rototranslations.size());
Eigen::Matrix4f result_mat = rototranslations[0];
Eigen::Affine3f affine(result_mat);
geometry_msgs::PoseStamped ros_pose;
tf::poseEigenToMsg(affine, ros_pose.pose);
ros_pose.header = scene_cloud_msg->header;
pub_output_.publish(ros_pose);
}
else {
NODELET_WARN("Failed to find object");
}
}
void callback(const PCMsg::ConstPtr& kinect_pc_msg){
// Conversion from sensor_msgs::PointCloud2 to pcl::PointCloud
pcl::fromROSMsg(*kinect_pc_msg, *kinects_pc_);
// Remove NaNs
vector<int> indices;
pcl::removeNaNFromPointCloud<PointType>(*kinects_pc_, *kinects_pc_, indices);
// Downsampling
pc_downsampling<PointType>(kinects_pc_, 0.005,cluster_cloud_);
// Get transform between pc frame and base_link
if(first_frame_){
// get transform between the pc frame and the output frame
get_transform(kinects_pc_->header.frame_id, "base_link", pc_transform_);
first_frame_ = false;
}
// Transform the pc to base_link
pcl::transformPointCloud(*cluster_cloud_, *cluster_cloud_, pc_transform_.translation, pc_transform_.rotation);
kinects_pc_->header.frame_id = "base_link";
// Clip the pointcloud
pcl::ConditionAnd<PointType>::Ptr height_cond (new pcl::ConditionAnd<PointType> ());
pcl::ConditionalRemoval<PointType> condrem (height_cond);
height_cond->addComparison ( pcl::FieldComparison<PointType>::ConstPtr (new pcl::FieldComparison<PointType> ("z", pcl::ComparisonOps::GT, 0.15)));
height_cond->addComparison ( pcl::FieldComparison<PointType>::ConstPtr (new pcl::FieldComparison<PointType> ("y", pcl::ComparisonOps::GT, -1.0)));
height_cond->addComparison ( pcl::FieldComparison<PointType>::ConstPtr (new pcl::FieldComparison<PointType> ("y", pcl::ComparisonOps::LT, 1.0)));
height_cond->addComparison ( pcl::FieldComparison<PointType>::ConstPtr (new pcl::FieldComparison<PointType> ("x", pcl::ComparisonOps::LT, 1.5)));
height_cond->addComparison ( pcl::FieldComparison<PointType>::ConstPtr (new pcl::FieldComparison<PointType> ("x", pcl::ComparisonOps::GT, -0.5)));
condrem.setInputCloud (cluster_cloud_);
condrem.setKeepOrganized(true);
condrem.filter (*cluster_cloud_);
// Estimate normals
norm_est_.setKSearch(20);
norm_est_.setInputCloud (cluster_cloud_);
norm_est_.compute (*normals_);
// PCL Viewer
if(first_frame_)
viewer_.addPointCloudNormals<PointType, NormalType>(cluster_cloud_, normals_, 1, 0.05, "normals");
else{
viewer_.removePointCloud("normals");
viewer_.addPointCloudNormals<PointType, NormalType>(cluster_cloud_, normals_, 1, 0.05, "normals");
}
viewer_.updatePointCloud(cluster_cloud_,"sample_cloud");
viewer_.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "normals");
viewer_.spinOnce();
bool use_hough_ (true), show_correspondences_(true), show_keypoints_(true);
float model_ss_ (0.005f);
float scene_ss_ (0.005f);
float descr_rad_ (0.036f);
//float rf_rad_ (0.1f);
float rf_rad_(rf_rad);
float cg_size_ (0.2f);
float cg_thresh_ (3.0f);
int knn = 1;
pcl::PointCloud<int> model_keypoints_indices, scene_keypoints_indices;
pcl::PointCloud<PointType>::Ptr model_keypoints, scene_keypoints;
model_keypoints = boost::shared_ptr<pcl::PointCloud<PointType> >(new pcl::PointCloud<PointType> );
scene_keypoints = boost::shared_ptr<pcl::PointCloud<PointType> >(new pcl::PointCloud<PointType> );
pcl::UniformSampling<PointType> uniform_sampling;
uniform_sampling.setInputCloud (robot_pc_);
uniform_sampling.setRadiusSearch (model_ss_);
uniform_sampling.compute(model_keypoints_indices);
std::cout << "Model total points: " << robot_pc_->size () << "; Selected Keypoints: " << model_keypoints_indices.size () << std::endl;
uniform_sampling.setInputCloud (cluster_cloud_);
uniform_sampling.setRadiusSearch (scene_ss_);;
uniform_sampling.compute(scene_keypoints_indices);
std::cout << "Scene total points: " << cluster_cloud_->size () << "; Selected Keypoints: " << scene_keypoints_indices.size () << std::endl;
// Use Keypoint indices to make a PointCloud
model_keypoints->points.resize(model_keypoints_indices.points.size ());
for (size_t i=0; i<model_keypoints_indices.points.size (); ++i)
model_keypoints->points[i].getVector3fMap () = robot_pc_->points[model_keypoints_indices.points[i]].getVector3fMap();
scene_keypoints->points.resize (scene_keypoints_indices.points.size ());
for (size_t i=0; i<scene_keypoints_indices.points.size (); ++i)
scene_keypoints->points[i].getVector3fMap () = cluster_cloud_->points[scene_keypoints_indices.points[i]].getVector3fMap();
std::cout<<"keypoints converted"<<std::endl;
//
// Compute Descriptor for keypoints
//
pcl::PointCloud<DescriptorType>::Ptr model_descriptors (new pcl::PointCloud<DescriptorType> ());
pcl::PointCloud<DescriptorType>::Ptr scene_descriptors (new pcl::PointCloud<DescriptorType> ());
pcl::SHOTEstimationOMP<PointType, NormalType, DescriptorType> descr_est;
//descr_est.setKSearch(10);
descr_est.setRadiusSearch (descr_rad_);
descr_est.setInputCloud (model_keypoints);
descr_est.setInputNormals (robot_normals_);
descr_est.setSearchSurface (robot_pc_);
descr_est.compute (*model_descriptors);
std::cout<<"SHOTEstimationOMP done for model"<<std::endl;
pcl::SHOTEstimationOMP<PointType, NormalType, DescriptorType> descr_est2;
// descr_est2.setKSearch(10);
descr_est2.setRadiusSearch (descr_rad_);
descr_est2.setInputCloud (scene_keypoints);
descr_est2.setInputNormals (normals_);
descr_est2.setSearchSurface (cluster_cloud_);
descr_est2.compute (*scene_descriptors);
std::cout<<"SHOTEstimationOMP done for scene"<<std::endl;
//
// Find Model-Scene Correspondences with KdTree
//
pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ());
pcl::KdTreeFLANN<DescriptorType> match_search;
match_search.setInputCloud (model_descriptors);
std::cout<<"KdTreeFLANN searched"<<std::endl;
// For each scene keypoint descriptor, find nearest neighbor into the model keypoints descriptor cloud and add it to the correspondences vector.
// for (size_t i = 0; i < scene_descriptors->size (); ++i)
// {
// std::vector<int> neigh_indices (1);
// std::vector<float> neigh_sqr_dists (1);
// if (!pcl_isfinite (scene_descriptors->at (i).descriptor[0])) //skipping NaNs
// {
// continue;
// }
// int found_neighs = match_search.nearestKSearch (scene_descriptors->at (i), 1, neigh_indices, neigh_sqr_dists);
// // if(found_neighs == 1 && neigh_sqr_dists[0] < 0.25f) // add match only if the squared descriptor distance is less than 0.25 (SHOT descriptor distances are between 0 and 1 by design)
// if(found_neighs == 1 && neigh_sqr_dists[0] < 0.25f) // add match only if the squared descriptor distance is less than 0.25 (SHOT descriptor distances are between 0 and 1 by design)
// {
// pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
// model_scene_corrs->push_back (corr);
// }
// }
for (size_t i = 0; i < scene_descriptors->size (); ++i)
{
std::vector<int> neigh_indices (knn);
std::vector<float> neigh_sqr_dists (knn);
if (!pcl_isfinite (scene_descriptors->at (i).descriptor[0])) //skipping NaNs
{
continue;
}
int found_neighs = match_search.nearestKSearch (scene_descriptors->at (i), knn, neigh_indices, neigh_sqr_dists);
for(int k = 0; k < found_neighs; k++)
{
if(found_neighs == 1 && neigh_sqr_dists[k] < 0.1f) // add match only if the squared descriptor distance is less than 0.25 (SHOT descriptor distances are between 0 and 1 by design)
{
pcl::Correspondence corr (neigh_indices[k], static_cast<int> (i), neigh_sqr_dists[k]);
model_scene_corrs->push_back (corr);
}
}
}
std::cout << "Correspondences found: " << model_scene_corrs->size () << std::endl;
//
// Actual Clustering
//
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
std::vector<pcl::Correspondences> clustered_corrs;
std::cout<<"Actual clustering done"<<std::endl;
// Using Hough3D
if (use_hough_)
{
//
// Compute (Keypoints) Reference Frames only for Hough
//
pcl::PointCloud<RFType>::Ptr model_rf (new pcl::PointCloud<RFType> ());
pcl::PointCloud<RFType>::Ptr scene_rf (new pcl::PointCloud<RFType> ());
pcl::BOARDLocalReferenceFrameEstimation<PointType, NormalType, RFType> rf_est;
rf_est.setFindHoles (true);
rf_est.setRadiusSearch (rf_rad_);
rf_est.setInputCloud (model_keypoints);
rf_est.setInputNormals (robot_normals_);
rf_est.setSearchSurface (robot_pc_);
rf_est.compute (*model_rf);
rf_est.setInputCloud (scene_keypoints);
rf_est.setInputNormals (normals_);
rf_est.setSearchSurface (cluster_cloud_);
rf_est.compute (*scene_rf);
// Clustering
pcl::Hough3DGrouping<PointType, PointType, RFType, RFType> clusterer;
clusterer.setHoughBinSize (cg_size_);
clusterer.setHoughThreshold (cg_thresh_);
clusterer.setUseInterpolation (true);
clusterer.setUseDistanceWeight (false);
clusterer.setInputCloud (model_keypoints);
clusterer.setInputRf (model_rf);
clusterer.setSceneCloud (scene_keypoints);
clusterer.setSceneRf (scene_rf);
clusterer.setModelSceneCorrespondences (model_scene_corrs);
//clusterer.cluster (clustered_corrs);
clusterer.recognize (rototranslations, clustered_corrs);
}
else // Using GeometricConsistency
{
pcl::GeometricConsistencyGrouping<PointType, PointType> gc_clusterer;
gc_clusterer.setGCSize (cg_size_);
gc_clusterer.setGCThreshold (cg_thresh_);
gc_clusterer.setInputCloud (model_keypoints);
gc_clusterer.setSceneCloud (scene_keypoints);
gc_clusterer.setModelSceneCorrespondences (model_scene_corrs);
//gc_clusterer.cluster (clustered_corrs);
gc_clusterer.recognize (rototranslations, clustered_corrs);
}
//
// Output results
//
std::cout << "Model instances found: " << rototranslations.size () << std::endl;
for (size_t i = 0; i < rototranslations.size (); ++i)
{
std::cout << "\n Instance " << i + 1 << ":" << std::endl;
std::cout << " Correspondences belonging to this instance: " << clustered_corrs[i].size () << std::endl;
// Print the rotation matrix and translation vector
Eigen::Matrix3f rotation = rototranslations[i].block<3,3>(0, 0);
Eigen::Vector3f translation = rototranslations[i].block<3,1>(0, 3);
printf ("\n");
printf (" | %6.3f %6.3f %6.3f | \n", rotation (0,0), rotation (0,1), rotation (0,2));
printf (" R = | %6.3f %6.3f %6.3f | \n", rotation (1,0), rotation (1,1), rotation (1,2));
printf (" | %6.3f %6.3f %6.3f | \n", rotation (2,0), rotation (2,1), rotation (2,2));
printf ("\n");
printf (" t = < %0.3f, %0.3f, %0.3f >\n", translation (0), translation (1), translation (2));
}
// Visualization
// White cloud: scene
// Yellow cloud: off_scene_MODEL
// Blue cloud: scene keypoints and off_scene_MODEL keypoints
// Red cloud: rotated model
// Green lines: correspondences between
pcl::visualization::PCLVisualizer viewer ("Correspondence Grouping");
viewer.addPointCloud (cluster_cloud_, "scene_cloud");
pcl::PointCloud<PointType>::Ptr off_scene_model (new pcl::PointCloud<PointType> ());
pcl::PointCloud<PointType>::Ptr off_scene_model_keypoints (new pcl::PointCloud<PointType> ());
if (show_correspondences_ || show_keypoints_)
{
// We are translating the model so that it doesn't end in the middle of the scene representation
pcl::transformPointCloud (*robot_pc_, *off_scene_model, Eigen::Vector3f (-1,0,0), Eigen::Quaternionf (1, 0, 0, 0));
pcl::transformPointCloud (*model_keypoints, *off_scene_model_keypoints, Eigen::Vector3f (-1,0,0), Eigen::Quaternionf (1, 0, 0, 0));
pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_color_handler (off_scene_model, 255, 255, 128);
viewer.addPointCloud (off_scene_model, off_scene_model_color_handler, "off_scene_model");
}
if (show_keypoints_)
{
pcl::visualization::PointCloudColorHandlerCustom<PointType> scene_keypoints_color_handler (scene_keypoints, 0, 0, 255);
viewer.addPointCloud (scene_keypoints, scene_keypoints_color_handler, "scene_keypoints");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "scene_keypoints");
pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_keypoints_color_handler (off_scene_model_keypoints, 0, 0, 255);
viewer.addPointCloud (off_scene_model_keypoints, off_scene_model_keypoints_color_handler, "off_scene_model_keypoints");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "off_scene_model_keypoints");
}
for (size_t i = 0; i < rototranslations.size (); ++i)
{
pcl::PointCloud<PointType>::Ptr rotated_model (new pcl::PointCloud<PointType> ());
pcl::transformPointCloud (*robot_pc_, *rotated_model, rototranslations[i]);
std::stringstream ss_cloud;
ss_cloud << "instance" << i;
pcl::visualization::PointCloudColorHandlerCustom<PointType> rotated_model_color_handler (rotated_model, 255, 0, 0);
viewer.addPointCloud (rotated_model, rotated_model_color_handler, ss_cloud.str ());
if (show_correspondences_)
{
for (size_t j = 0; j < clustered_corrs[i].size (); ++j)
{
std::stringstream ss_line;
ss_line << "correspondence_line" << i << "_" << j;
PointType& model_point = off_scene_model_keypoints->at (clustered_corrs[i][j].index_query);
PointType& scene_point = scene_keypoints->at (clustered_corrs[i][j].index_match);
// We are drawing a line for each pair of clustered correspondences found between the model and the scene
viewer.addLine<PointType, PointType> (model_point, scene_point, 0, 255, 0, ss_line.str ());
}
}
}
while (!viewer.wasStopped ())
{
viewer.spinOnce ();
}
exit(0);
}
int
correspondenceGroup ()
{
// Load clouds
loadCloud ();
// Compute Cloud Resolution
computeCloudResolution(model);
// Compute Normals
pcl::NormalEstimationOMP<PointType, NormalType> norm_est;
norm_est.setKSearch (10);
norm_est.setInputCloud (model);
norm_est.compute (*model_normals);
norm_est.setInputCloud (scene);
norm_est.compute (*scene_normals);
// Downsample Clouds to Extract keypoints
pcl::PointCloud<int> sampled_indices;
pcl::UniformSampling<PointType> uniform_sampling;
uniform_sampling.setInputCloud (model);
uniform_sampling.setRadiusSearch (model_ss_);
uniform_sampling.compute (sampled_indices);
pcl::copyPointCloud (*model, sampled_indices.points, *model_keypoints);
std::cout << "Model total points: " << model->size () << "; Selected Keypoints: " << model_keypoints->size () << std::endl;
uniform_sampling.setInputCloud (scene);
uniform_sampling.setRadiusSearch (scene_ss_);
uniform_sampling.compute (sampled_indices);
pcl::copyPointCloud (*scene, sampled_indices.points, *scene_keypoints);
std::cout << "Scene total points: " << scene->size () << "; Selected Keypoints: " << scene_keypoints->size () << std::endl;
// Compute Descriptor for keypoints
pcl::SHOTEstimationOMP<PointType, NormalType, DescriptorType> descr_est;
descr_est.setRadiusSearch (descr_rad_);
descr_est.setInputCloud (model_keypoints);
descr_est.setInputNormals (model_normals);
descr_est.setSearchSurface (model);
descr_est.compute (*model_descriptors);
descr_est.setInputCloud (scene_keypoints);
descr_est.setInputNormals (scene_normals);
descr_est.setSearchSurface (scene);
descr_est.compute (*scene_descriptors);
// Find Model-Scene Correspondences with KdTree
pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ());
pcl::KdTreeFLANN<DescriptorType> match_search;
match_search.setInputCloud (model_descriptors);
// For each scene keypoint descriptor, find nearest neighbor into the model keypoints descriptor cloud and add it to the correspondences vector.
for (size_t i = 0; i < scene_descriptors->size (); ++i)
{
std::vector<int> neigh_indices (1);
std::vector<float> neigh_sqr_dists (1);
if (!pcl_isfinite (scene_descriptors->at (i).descriptor[0])) //skipping NaNs
{
continue;
}
int found_neighs = match_search.nearestKSearch (scene_descriptors->at (i), 1, neigh_indices, neigh_sqr_dists);
if(found_neighs == 1 && neigh_sqr_dists[0] < 0.25f) // add match only if the squared descriptor distance is less than 0.25 (SHOT descriptor distances are between 0 and 1 by design)
{
pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
model_scene_corrs->push_back (corr);
}
}
std::cout << "Correspondences found: " << model_scene_corrs->size () << std::endl;
// Actual Clustering
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
std::vector<pcl::Correspondences> clustered_corrs;
// Using Hough3D
if (use_hough_)
{
//
// Compute (Keypoints) Reference Frames only for Hough
//
pcl::PointCloud<RFType>::Ptr model_rf (new pcl::PointCloud<RFType> ());
pcl::PointCloud<RFType>::Ptr scene_rf (new pcl::PointCloud<RFType> ());
pcl::BOARDLocalReferenceFrameEstimation<PointType, NormalType, RFType> rf_est;
rf_est.setFindHoles (true);
rf_est.setRadiusSearch (rf_rad_);
rf_est.setInputCloud (model_keypoints);
rf_est.setInputNormals (model_normals);
rf_est.setSearchSurface (model);
rf_est.compute (*model_rf);
rf_est.setInputCloud (scene_keypoints);
rf_est.setInputNormals (scene_normals);
rf_est.setSearchSurface (scene);
rf_est.compute (*scene_rf);
// Clustering
pcl::Hough3DGrouping<PointType, PointType, RFType, RFType> clusterer;
clusterer.setHoughBinSize (cg_size_);
clusterer.setHoughThreshold (cg_thresh_);
clusterer.setUseInterpolation (true);
clusterer.setUseDistanceWeight (false);
clusterer.setInputCloud (model_keypoints);
clusterer.setInputRf (model_rf);
clusterer.setSceneCloud (scene_keypoints);
clusterer.setSceneRf (scene_rf);
clusterer.setModelSceneCorrespondences (model_scene_corrs);
//clusterer.cluster (clustered_corrs);
clusterer.recognize (rototranslations, clustered_corrs);
}
else // Using GeometricConsistency
{
pcl::GeometricConsistencyGrouping<PointType, PointType> gc_clusterer;
gc_clusterer.setGCSize (cg_size_);
gc_clusterer.setGCThreshold (cg_thresh_);
gc_clusterer.setInputCloud (model_keypoints);
gc_clusterer.setSceneCloud (scene_keypoints);
gc_clusterer.setModelSceneCorrespondences (model_scene_corrs);
//gc_clusterer.cluster (clustered_corrs);
gc_clusterer.recognize (rototranslations, clustered_corrs);
}
// Output results
std::cout << "Model instances found: " << rototranslations.size () << std::endl;
/*for (size_t i = 0; i < rototranslations.size (); ++i)
{
std::cout << "\n Instance " << i + 1 << ":" << std::endl;
std::cout << " Correspondences belonging to this instance: " << clustered_corrs[i].size () << std::endl;
// Print the rotation matrix and translation vector
Eigen::Matrix3f rotation = rototranslations[i].block<3,3>(0, 0);
Eigen::Vector3f translation = rototranslations[i].block<3,1>(0, 3);
printf ("\n");
printf (" | %6.3f %6.3f %6.3f | \n", rotation (0,0), rotation (0,1), rotation (0,2));
printf (" R = | %6.3f %6.3f %6.3f | \n", rotation (1,0), rotation (1,1), rotation (1,2));
printf (" | %6.3f %6.3f %6.3f | \n", rotation (2,0), rotation (2,1), rotation (2,2));
printf ("\n");
printf (" t = < %0.3f, %0.3f, %0.3f >\n", translation (0), translation (1), translation (2));
}*/
return rototranslations.size();
}