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