/**
* p = R*x + T
* if inverse:
* x = R^1*(p - T)
*/
inline Eigen::Affine3f
RT2Transform(cv::Mat& R, cv::Mat& T, bool inverse)
{
//convert the tranform from our fiducial markers to
//the Eigen
Eigen::Matrix<float, 3, 3> eR;
Eigen::Vector3f eT;
cv::cv2eigen(R, eR);
cv::cv2eigen(T, eT);
// p = R*x + T
Eigen::Affine3f transform;
if (inverse)
{
//x = R^1*(p - T)
transform = Eigen::Translation3f(-eT);
transform.prerotate(eR.transpose());
}
else
{
//p = R*x + T
transform = Eigen::AngleAxisf(eR);
transform.translate(eT);
}
return transform;
}
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);
}