shot_detector::shot_detector()
{
data = false;
activated = false;
std::cerr << "Starting" << std::endl;
// Start the ros stuff such as the subscriber and the service
nh = ros::NodeHandle("apc_object_detection");
loadParameters();
//kinect=nh.subscribe("/kinect2_cool/depth_highres/points", 1, &shot_detector::PointCloudCallback,this);
processor = nh.advertiseService("Shot_detector", &shot_detector::processCloud, this);
// As we use Ptr to access our pointcloud we first have to initalize something to point to
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> ());
pcl::CorrespondencesPtr model_scene_corrs_ (new pcl::Correspondences ());
pcl::PointCloud<PointType>::Ptr model_good_kp_ (new pcl::PointCloud<PointType> ());
pcl::PointCloud<PointType>::Ptr scene_good_kp_ (new pcl::PointCloud<PointType> ());
pcl::PointCloud<PointType>::Ptr objects_ (new pcl::PointCloud<PointType> ());
//pcl::PointCloud<DescriptorType>::Ptr model_descriptors_ (new pcl::PointCloud<DescriptorType> ());
//pcl::PointCloud<DescriptorType>::Ptr scene_descriptors_ (new pcl::PointCloud<DescriptorType> ());
model = model_;
model_keypoints = model_keypoints_;
scene = scene_;
scene_keypoints = scene_keypoints_;
model_normals = model_normals_;
scene_normals = scene_normals_;
model_descriptors = model_descriptors_;
scene_descriptors = scene_descriptors_;
model_scene_corrs = model_scene_corrs_;
model_good_kp = model_good_kp_;
scene_good_kp = scene_good_kp_;
objects = objects_;
pcl::CorrespondencesPtr correspondences (new pcl::Correspondences);
correspondences_ = correspondences;
//Set up a couple of the pcl settings
descr_est.setRadiusSearch (descr_rad_);
norm_est.setKSearch (10);
//calcPFHRGBDescriptors(model,model_keypoints,model_normals,model_descriptors);
}
void kpoAnalyzerThread::operator ()()
{
std::cout << "cloud has " << scene_cloud_->size() << " points" << std::endl;
Cloud cleanCloud;
removeNoise(scene_cloud_, cleanCloud);
pcl::copyPointCloud(cleanCloud, *scene_cloud_);
std::cout << "filtered cloud has " << scene_cloud_->size() << " points" << std::endl;
/*
if (scene_cloud_->size() < 25) {
std::cout << "cloud too small" << std::endl;
return;
}
/*
if (scene_cloud_->size() > 40000) {
std::cout << "cloud too large" << std::endl;
return;
}
*/
NormalCloud::Ptr scene_normals_(new NormalCloud());
estimateNormals(scene_cloud_, scene_normals_);
Cloud::Ptr scene_keypoints_(new Cloud());
downSample(scene_cloud_, scene_keypoints_);
DescriptorCloud::Ptr scene_descriptors_(new DescriptorCloud());
computeShotDescriptors(scene_cloud_, scene_keypoints_, scene_normals_, scene_descriptors_);
RFCloud::Ptr scene_refs_(new RFCloud());
estimateReferenceFrames(scene_cloud_, scene_normals_, scene_keypoints_, scene_refs_);
kpoCloudDescription od;
od.cloud = *scene_cloud_;
od.keypoints = *scene_keypoints_;
od.normals = *scene_normals_;
od.descriptors = *scene_descriptors_;
od.reference_frames = *scene_refs_;
od.filename = filename;
od.object_id = object_id;
callback_(od);
}
