void
LocalRecognitionPipeline<PointT>::correspondenceGrouping ()
{
if(cg_algorithm_->getRequiresNormals() && (!scene_normals_ || scene_normals_->points.size() != scene_->points.size()))
computeNormals<PointT>(scene_, scene_normals_, param_.normal_computation_method_);
typename std::map<std::string, ObjectHypothesis<PointT> >::iterator it;
for (it = obj_hypotheses_.begin (); it != obj_hypotheses_.end (); it++)
{
ObjectHypothesis<PointT> &oh = it->second;
if(oh.model_scene_corresp_.size() < 3)
continue;
std::vector < pcl::Correspondences > corresp_clusters;
cg_algorithm_->setSceneCloud (scene_);
cg_algorithm_->setInputCloud (oh.model_->keypoints_);
if(cg_algorithm_->getRequiresNormals())
cg_algorithm_->setInputAndSceneNormals(oh.model_->kp_normals_, scene_normals_);
//we need to pass the keypoints_pointcloud and the specific object hypothesis
cg_algorithm_->setModelSceneCorrespondences (oh.model_scene_corresp_);
cg_algorithm_->cluster (corresp_clusters);
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > new_transforms (corresp_clusters.size());
typename pcl::registration::TransformationEstimationSVD < PointT, PointT > t_est;
for (size_t i = 0; i < corresp_clusters.size(); i++)
t_est.estimateRigidTransformation (*oh.model_->keypoints_, *scene_, corresp_clusters[i], new_transforms[i]);
if(param_.merge_close_hypotheses_) {
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > merged_transforms (corresp_clusters.size());
std::vector<bool> cluster_has_been_taken(corresp_clusters.size(), false);
const double angle_thresh_rad = param_.merge_close_hypotheses_angle_ * M_PI / 180.f ;
size_t kept=0;
for (size_t i = 0; i < new_transforms.size(); i++) {
if (cluster_has_been_taken[i])
continue;
cluster_has_been_taken[i] = true;
const Eigen::Vector3f centroid1 = new_transforms[i].block<3, 1> (0, 3);
const Eigen::Matrix3f rot1 = new_transforms[i].block<3, 3> (0, 0);
pcl::Correspondences merged_corrs = corresp_clusters[i];
for(size_t j=i; j < new_transforms.size(); j++) {
const Eigen::Vector3f centroid2 = new_transforms[j].block<3, 1> (0, 3);
const Eigen::Matrix3f rot2 = new_transforms[j].block<3, 3> (0, 0);
const Eigen::Matrix3f rot_diff = rot2 * rot1.transpose();
double rotx = atan2(rot_diff(2,1), rot_diff(2,2));
double roty = atan2(-rot_diff(2,0), sqrt(rot_diff(2,1) * rot_diff(2,1) + rot_diff(2,2) * rot_diff(2,2)));
double rotz = atan2(rot_diff(1,0), rot_diff(0,0));
double dist = (centroid1 - centroid2).norm();
if ( (dist < param_.merge_close_hypotheses_dist_) && (rotx < angle_thresh_rad) && (roty < angle_thresh_rad) && (rotz < angle_thresh_rad) ) {
merged_corrs.insert( merged_corrs.end(), corresp_clusters[j].begin(), corresp_clusters[j].end() );
cluster_has_been_taken[j] = true;
}
}
t_est.estimateRigidTransformation (*oh.model_->keypoints_, *scene_, merged_corrs, merged_transforms[kept]);
kept++;
}
merged_transforms.resize(kept);
new_transforms = merged_transforms;
}
std::cout << "Merged " << corresp_clusters.size() << " clusters into " << new_transforms.size() << " clusters. Total correspondences: " << oh.model_scene_corresp_.size () << " " << it->first << std::endl;
// oh.visualize(*scene_);
size_t existing_hypotheses = models_.size();
models_.resize( existing_hypotheses + new_transforms.size(), oh.model_ );
transforms_.insert(transforms_.end(), new_transforms.begin(), new_transforms.end());
}
}
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;
}
}