/**
      * 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;
 }
Пример #2
0
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);
}