void shot_detector::processImage()
{
std::cerr << "Processing" << std::endl;
std::string file = "/home/niko/projects/apc/catkin/src/apc_ros/apc_object_detection/visual_hull_refined_smoothed.obj";
loadModel(model, file);
pcl::io::loadPCDFile("/home/niko/projects/apc/catkin/src/apc_ros/apc_object_detection/niko_file.pcd", *scene);
//Downsample the model and the scene so they have rougly the same resolution
pcl::PointCloud<PointType>::Ptr scene_filter (new pcl::PointCloud<PointType> ());
pcl_functions::voxelFilter(scene, scene_filter, voxel_sample_);
scene = scene_filter;
pcl::PointCloud<PointType>::Ptr model_filter (new pcl::PointCloud<PointType> ());
pcl_functions::voxelFilter(model, model_filter, voxel_sample_);
model = model_filter;
// Randomly select a couple of keypoints so we don't calculte descriptors for everything
sampleKeypoints(model, model_keypoints, model_ss_);
sampleKeypoints(scene, scene_keypoints, scene_ss_);
//Calculate the Normals
calcNormals(model, model_normals);
calcNormals(scene, scene_normals);
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
ourcvfh.setInputCloud(scene);
ourcvfh.setInputNormals(scene_normals);
ourcvfh.setSearchMethod(kdtree);
ourcvfh.setEPSAngleThreshold(5.0 / 180.0 * M_PI); // 5 degrees.
ourcvfh.setCurvatureThreshold(1.0);
ourcvfh.setNormalizeBins(false);
// Set the minimum axis ratio between the SGURF axes. At the disambiguation phase,
// this will decide if additional Reference Frames need to be created, if ambiguous.
ourcvfh.setAxisRatio(0.8);
ourcvfh.compute(vfh_scene_descriptors);
ourcvfh.setInputCloud(model);
ourcvfh.setInputNormals(model_normals);
ourcvfh.compute(vfh_model_descriptors);
//Calculate the shot descriptors at each keypoint in the scene
calcSHOTDescriptors(model, model_keypoints, model_normals, model_descriptors);
calcSHOTDescriptors(scene, scene_keypoints, scene_normals, scene_descriptors);
// Compare descriptors and try to find correspondences
//ransac(rototranslations,model,scene);
//refinePose(rototranslations,model,scene);
compare(model_descriptors, scene_descriptors);
groupCorrespondences();
visualizeCorrespondences();
visualizeICP();
/*Eigen::Matrix4f pose;
if(model_scene_corrs->size ()!=0){
groupCorrespondences();
ransac(rototranslations,model,scene);
pose=refinePose(rototranslations,model,scene);
}*/
}
void CModelQuery2D::computeAll()
{
pcl::StopWatch timer;
modelFeatures_.computeAll();
queryFeatures_.computeAll();
computeMatches();
computePoseEstimation();
perfData & pd = pGlobalPerf_->pd[descriptorType_];
pd.processingTime = timer.getTimeSeconds();
if ( inputParams_.debug_level == OUTPUT_RUNTIME_PERF )
{
pcl::console::print_info ("2D - DescID: %d, Time: %f\n", descriptorType_, pd.processingTime );
}
visualizeCorrespondences();
}
