/** estimate the normals of a point cloud */
static PointCloud<Normal>::Ptr
compute_pcn(PointCloud<PointXYZ>::ConstPtr in, float vx, float vy, float vz)
{
PointCloud<Normal>::Ptr pcn (new PointCloud<Normal>());
NormalEstimation<PointXYZ, Normal> ne;
search::KdTree<PointXYZ>::Ptr kdt (new search::KdTree<PointXYZ>());
ne.setInputCloud(in);
ne.setSearchMethod(kdt);
ne.setKSearch(20);
ne.setViewPoint(vx, vy, vz);
ne.compute(*pcn);
return pcn;
}
// Subsample cloud for faster matching and processing, while filling in normals.
void PointcloudProc::reduceCloud(const PointCloud<PointXYZRGB>& input, PointCloud<PointXYZRGBNormal>& output) const
{
PointCloud<PointXYZRGB> cloud_nan_filtered, cloud_box_filtered, cloud_voxel_reduced;
PointCloud<Normal> normals;
PointCloud<PointXYZRGBNormal> cloud_normals;
std::vector<int> indices;
// Filter out nans.
removeNaNFromPointCloud(input, cloud_nan_filtered, indices);
indices.clear();
// Filter out everything outside a [200x200x200] box.
Eigen::Vector4f min_pt(-100, -100, -100, -100);
Eigen::Vector4f max_pt(100, 100, 100, 100);
getPointsInBox(cloud_nan_filtered, min_pt, max_pt, indices);
ExtractIndices<PointXYZRGB> boxfilter;
boxfilter.setInputCloud(boost::make_shared<const PointCloud<PointXYZRGB> >(cloud_nan_filtered));
boxfilter.setIndices (boost::make_shared<vector<int> > (indices));
boxfilter.filter(cloud_box_filtered);
// Reduce pointcloud
VoxelGrid<PointXYZRGB> voxelfilter;
voxelfilter.setInputCloud (boost::make_shared<const PointCloud<PointXYZRGB> > (cloud_box_filtered));
voxelfilter.setLeafSize (0.05, 0.05, 0.05);
// voxelfilter.setLeafSize (0.1, 0.1, 0.1);
voxelfilter.filter (cloud_voxel_reduced);
// Compute normals
NormalEstimation<PointXYZRGB, Normal> normalest;
normalest.setViewPoint(0, 0, 0);
normalest.setSearchMethod (boost::make_shared<search::KdTree<PointXYZRGB> > ());
//normalest.setKSearch (10);
normalest.setRadiusSearch (0.25);
// normalest.setRadiusSearch (0.4);
normalest.setInputCloud(boost::make_shared<const PointCloud<PointXYZRGB> >(cloud_voxel_reduced));
normalest.compute(normals);
pcl::concatenateFields (cloud_voxel_reduced, normals, cloud_normals);
// Filter based on curvature
PassThrough<PointXYZRGBNormal> normalfilter;
normalfilter.setFilterFieldName("curvature");
// normalfilter.setFilterLimits(0.0, 0.2);
normalfilter.setFilterLimits(0.0, 0.2);
normalfilter.setInputCloud(boost::make_shared<const PointCloud<PointXYZRGBNormal> >(cloud_normals));
normalfilter.filter(output);
}
TEST (PCL, GSHOTWithRTransNoised)
{
PointCloud<PointNormal>::Ptr cloud_nr (new PointCloud<PointNormal> ());
PointCloud<PointNormal>::Ptr cloud_rot (new PointCloud<PointNormal> ());
PointCloud<PointNormal>::Ptr cloud_trans (new PointCloud<PointNormal> ());
PointCloud<PointNormal>::Ptr cloud_rot_trans (new PointCloud<PointNormal> ());
PointCloud<PointXYZ>::Ptr cloud_noise (new PointCloud<PointXYZ> ());
Eigen::Affine3f rot = Eigen::Affine3f::Identity ();
float rot_x = static_cast <float> (rand ()) / static_cast <float> (RAND_MAX);
float rot_y = static_cast <float> (rand ()) / static_cast <float> (RAND_MAX);
float rot_z = static_cast <float> (rand ()) / static_cast <float> (RAND_MAX);
rot.prerotate (Eigen::AngleAxisf (rot_x * M_PI, Eigen::Vector3f::UnitX ()));
rot.prerotate (Eigen::AngleAxisf (rot_y * M_PI, Eigen::Vector3f::UnitY ()));
rot.prerotate (Eigen::AngleAxisf (rot_z * M_PI, Eigen::Vector3f::UnitZ ()));
//std::cout << "rot = (" << (rot_x * M_PI) << ", " << (rot_y * M_PI) << ", " << (rot_z * M_PI) << ")" << std::endl;
Eigen::Affine3f trans = Eigen::Affine3f::Identity ();
float HI = 5.0f;
float LO = -HI;
float trans_x = LO + static_cast<float> (rand ()) / (static_cast<float> (RAND_MAX / (HI - LO)));
float trans_y = LO + static_cast<float> (rand ()) / (static_cast<float> (RAND_MAX / (HI - LO)));
float trans_z = LO + static_cast<float> (rand ()) / (static_cast<float> (RAND_MAX / (HI - LO)));
//std::cout << "trans = (" << trans_x << ", " << trans_y << ", " << trans_z << ")" << std::endl;
trans.translate (Eigen::Vector3f (trans_x, trans_y, trans_z));
// Estimate normals first
float mr = 0.002;
NormalEstimation<PointXYZ, pcl::Normal> n;
PointCloud<Normal>::Ptr normals1 (new PointCloud<Normal> ());
n.setViewPoint (0.0, 0.0, 1.0);
n.setInputCloud (cloud.makeShared ());
n.setRadiusSearch (20 * mr);
n.compute (*normals1);
pcl::concatenateFields<PointXYZ, Normal, PointNormal> (cloud, *normals1, *cloud_nr);
pcl::transformPointCloudWithNormals<PointNormal, float> (*cloud_nr, *cloud_rot, rot);
pcl::transformPointCloudWithNormals<PointNormal, float> (*cloud_nr, *cloud_trans, trans);
pcl::transformPointCloudWithNormals<PointNormal, float> (*cloud_rot, *cloud_rot_trans, trans);
add_gaussian_noise (cloud.makeShared (), cloud_noise, 0.005);
PointCloud<Normal>::Ptr normals_noise (new PointCloud<Normal> ());
n.setInputCloud (cloud_noise);
n.compute (*normals_noise);
PointCloud<Normal>::Ptr normals2 (new PointCloud<Normal> ());
n.setInputCloud (cloud2.makeShared ());
n.compute (*normals2);
PointCloud<Normal>::Ptr normals3 (new PointCloud<Normal> ());
n.setInputCloud (cloud3.makeShared ());
n.compute (*normals3);
// Objects
// Descriptors for ground truth (using SHOT)
PointCloud<SHOT352>::Ptr desc01 (new PointCloud<SHOT352> ());
PointCloud<SHOT352>::Ptr desc02 (new PointCloud<SHOT352> ());
PointCloud<SHOT352>::Ptr desc03 (new PointCloud<SHOT352> ());
PointCloud<SHOT352>::Ptr desc04 (new PointCloud<SHOT352> ());
// Descriptors for test GSHOT
PointCloud<SHOT352>::Ptr desc1 (new PointCloud<SHOT352> ());
PointCloud<SHOT352>::Ptr desc2 (new PointCloud<SHOT352> ());
PointCloud<SHOT352>::Ptr desc3 (new PointCloud<SHOT352> ());
PointCloud<SHOT352>::Ptr desc4 (new PointCloud<SHOT352> ());
PointCloud<SHOT352>::Ptr desc5 (new PointCloud<SHOT352> ());
PointCloud<SHOT352>::Ptr desc6 (new PointCloud<SHOT352> ());
PointCloud<SHOT352>::Ptr desc7 (new PointCloud<SHOT352> ());
// SHOT352 (global)
GSHOTEstimation<PointNormal, PointNormal, SHOT352> gshot1;
gshot1.setInputNormals (cloud_nr);
gshot1.setInputCloud (cloud_nr);
gshot1.compute (*desc1);
// Eigen::Vector4f center_desc1 = gshot.getCentralPoint ();
gshot1.setInputNormals (cloud_rot);
gshot1.setInputCloud (cloud_rot);
gshot1.compute (*desc2);
// Eigen::Vector4f center_desc2 = gshot.getCentralPoint ();
gshot1.setInputNormals (cloud_trans);
gshot1.setInputCloud (cloud_trans);
gshot1.compute (*desc3);
// Eigen::Vector4f center_desc3 = gshot.getCentralPoint ();
gshot1.setInputNormals (cloud_rot_trans);
gshot1.setInputCloud (cloud_rot_trans);
gshot1.compute (*desc4);
// Eigen::Vector4f center_desc4 = gshot.getCentralPoint ();
GSHOTEstimation<PointXYZ, Normal, SHOT352> gshot2;
gshot2.setInputNormals (normals1);
gshot2.setInputCloud (cloud_noise);
gshot2.compute (*desc5);
gshot2.setInputNormals (normals2);
gshot2.setInputCloud (cloud2.makeShared ());
gshot2.compute (*desc6);
gshot2.setInputNormals (normals3);
gshot2.setInputCloud (cloud3.makeShared ());
gshot2.compute (*desc7);
// Eigen::Vector3f distance_desc = (center_desc3.head<3> () - center_desc1.head<3> ());
// std::cout << "dist of desc0 and desc3 -> (" << distance_desc[0] << ", " << distance_desc[1] << ", " << distance_desc[2] << ")\n";
// SHOT352 (local)
GSHOTEstimation<PointNormal, PointNormal, SHOT352> shot;
shot.setInputNormals (cloud_nr);
shot.setInputCloud (ground_truth.makeShared());
shot.setSearchSurface (cloud_nr);
shot.setRadiusSearch (radius_local_shot);
shot.compute (*desc01);
shot.setInputNormals (cloud_rot);
shot.setInputCloud (ground_truth.makeShared());
shot.setSearchSurface (cloud_rot);
shot.setRadiusSearch (radius_local_shot);
shot.compute (*desc02);
shot.setInputNormals (cloud_trans);
shot.setInputCloud (ground_truth.makeShared());
shot.setSearchSurface (cloud_trans);
shot.setRadiusSearch (radius_local_shot);
shot.compute (*desc03);
shot.setInputNormals (cloud_rot_trans);
shot.setInputCloud (ground_truth.makeShared());
shot.setSearchSurface (cloud_rot_trans);
shot.setRadiusSearch (radius_local_shot);
shot.compute (*desc04);
// CHECK GSHOT
checkDesc(*desc01, *desc1);
checkDesc(*desc02, *desc2);
checkDesc(*desc03, *desc3);
checkDesc(*desc04, *desc4);
std::vector<float> d0, d1, d2, d3, d4, d5, d6;
for(int i = 0; i < 352; ++i)
{
d0.push_back(desc1->points[0].descriptor[i]);
d1.push_back(desc2->points[0].descriptor[i]);
d2.push_back(desc3->points[0].descriptor[i]);
d3.push_back(desc4->points[0].descriptor[i]);
d4.push_back(desc5->points[0].descriptor[i]);
d5.push_back(desc6->points[0].descriptor[i]);
d6.push_back(desc7->points[0].descriptor[i]);
}
float dist_0 = pcl::selectNorm< std::vector<float> > (d0, d0, 352, pcl::HIK) ;
float dist_1 = pcl::selectNorm< std::vector<float> > (d0, d1, 352, pcl::HIK) ;
float dist_2 = pcl::selectNorm< std::vector<float> > (d0, d2, 352, pcl::HIK) ;
float dist_3 = pcl::selectNorm< std::vector<float> > (d0, d3, 352, pcl::HIK) ;
float dist_4 = pcl::selectNorm< std::vector<float> > (d0, d4, 352, pcl::HIK) ;
float dist_5 = pcl::selectNorm< std::vector<float> > (d0, d5, 352, pcl::HIK) ;
float dist_6 = pcl::selectNorm< std::vector<float> > (d0, d6, 352, pcl::HIK) ;
std::cout << ">> Itself[HIK]: " << dist_0 << std::endl
<< ">> Rotation[HIK]: " << dist_1 << std::endl
<< ">> Translate[HIK]: " << dist_2 << std::endl
<< ">> Rot+Trans[HIK] " << dist_3 << std::endl
<< ">> GaussNoise[HIK]: " << dist_4 << std::endl
<< ">> bun03[HIK]: " << dist_5 << std::endl
<< ">> milk[HIK]: " << dist_6 << std::endl;
float high_barrier = dist_0 * 0.999f;
float noise_barrier = dist_0 * 0.75f;
float cut_barrier = dist_0 * 0.20f;
float low_barrier = dist_0 * 0.02f;
EXPECT_GT (dist_1, high_barrier);
EXPECT_GT (dist_2, high_barrier);
//EXPECT_GT (dist_3, high_barrier);
EXPECT_GT (dist_4, noise_barrier);
EXPECT_GT (dist_5, cut_barrier);
EXPECT_LT (dist_6, low_barrier);
}