예제 #1
1
파일: test_pca.cpp 프로젝트: nttputus/PCL
TEST(PCA, projection)
{
  pcl::PointXYZ projected, reconstructed;
  for(size_t i = 0; i < cloud.size(); i++)
  {
    pca.project (cloud[i], projected);
    pca.reconstruct (projected, reconstructed);
    EXPECT_NEAR_VECTORS (reconstructed.getVector3fMap (), cloud[i].getVector3fMap (), 2.5e-4);
  }
}
예제 #2
0
TEST(PCA, projection)
{
  pcl::PointXYZ projected, reconstructed;
  for(const auto &point : cloud)
  {
    pca.project (point, projected);
    pca.reconstruct (projected, reconstructed);
    EXPECT_NEAR_VECTORS (reconstructed.getVector3fMap (), point.getVector3fMap (), 2.5e-4);
  }
}
예제 #3
0
파일: test_pca.cpp 프로젝트: nttputus/PCL
int main (int argc, char** argv)
{
  cloud.width = 5;
  cloud.height = 4 ;
  cloud.is_dense = true;
  cloud.resize(20);
  cloud[0].x = 100;   cloud[0].y = 8;    cloud[0].z = 5;
  cloud[1].x = 228;   cloud[1].y = 21;   cloud[1].z = 2;
  cloud[2].x = 341;   cloud[2].y = 31;   cloud[2].z = 10;
  cloud[3].x = 472;   cloud[3].y = 40;   cloud[3].z = 15;
  cloud[4].x = 578;   cloud[4].y = 48;   cloud[4].z = 3;
  cloud[5].x = 699;   cloud[5].y = 60;   cloud[5].z = 12;
  cloud[6].x = 807;   cloud[6].y = 71;   cloud[6].z = 14;
  cloud[7].x = 929;   cloud[7].y = 79;   cloud[7].z = 16;
  cloud[8].x = 1040;  cloud[8].y = 92;   cloud[8].z = 18;
  cloud[9].x = 1160;  cloud[9].y = 101;  cloud[9].z = 38;
  cloud[10].x = 1262; cloud[10].y = 109; cloud[10].z = 28;
  cloud[11].x = 1376; cloud[11].y = 121; cloud[11].z = 32;
  cloud[12].x = 1499; cloud[12].y = 128; cloud[12].z = 35;
  cloud[13].x = 1620; cloud[13].y = 143; cloud[13].z = 28;
  cloud[14].x = 1722; cloud[14].y = 150; cloud[14].z = 30;
  cloud[15].x = 1833; cloud[15].y = 159; cloud[15].z = 15;
  cloud[16].x = 1948; cloud[16].y = 172; cloud[16].z = 12;
  cloud[17].x = 2077; cloud[17].y = 181; cloud[17].z = 33;
  cloud[18].x = 2282; cloud[18].y = 190; cloud[18].z = 23;
  cloud[19].x = 2999; cloud[19].y = 202; cloud[19].z = 29;  

  pca.setInputCloud (cloud.makeShared ());

  testing::InitGoogleTest (&argc, argv);
  return (RUN_ALL_TESTS ());
}
예제 #4
0
파일: test_pca.cpp 프로젝트: nttputus/PCL
TEST(PCA, cloud_projection)
{
  pcl::PointCloud<pcl::PointXYZ> cloud_projected, cloud_reconstructed;
  try
  {
    pca.project (cloud, cloud_projected);
    EXPECT_EQ (cloud.size (), cloud_projected.size ());
    pca.reconstruct (cloud_projected, cloud_reconstructed);
    EXPECT_EQ (cloud_reconstructed.size (), cloud_projected.size ());
    for(size_t i = 0; i < cloud.size(); i++)
      EXPECT_NEAR_VECTORS (cloud[i].getVector3fMap (),
                           cloud_reconstructed[i].getVector3fMap (),
                           2.5e-4);
  }
  catch (pcl::InitFailedException &e)
  {
    std::cerr << "something wrong" << std::endl;
  }
}
void pushEigenMarker(pcl::PCA<T>& pca,
                     int& marker_id,
                     MarkerArray& marker_array,
                     double scale,
                     const std::string& frame_id)
{
  Marker marker;
  marker.lifetime = ros::Duration(0.1);
  marker.header.frame_id = frame_id;
  marker.ns = "cluster_eigen";
  marker.type = Marker::ARROW;
  marker.scale.y = 0.01;
  marker.scale.z = 0.01;
  marker.pose.position.x = pca.getMean().coeff(0);
  marker.pose.position.y = pca.getMean().coeff(1);
  marker.pose.position.z = pca.getMean().coeff(2);

  Eigen::Quaternionf qx, qy, qz;
  Eigen::Matrix3f ev = pca.getEigenVectors();
  Eigen::Vector3f axis_x(ev.coeff(0, 0), ev.coeff(1, 0), ev.coeff(2, 0));
  Eigen::Vector3f axis_y(ev.coeff(0, 1), ev.coeff(1, 1), ev.coeff(2, 1));
  Eigen::Vector3f axis_z(ev.coeff(0, 2), ev.coeff(1, 2), ev.coeff(2, 2));
  qx.setFromTwoVectors(Eigen::Vector3f(1, 0, 0), axis_x);
  qy.setFromTwoVectors(Eigen::Vector3f(1, 0, 0), axis_y);
  qz.setFromTwoVectors(Eigen::Vector3f(1, 0, 0), axis_z);

  marker.id = marker_id++;
  marker.scale.x = pca.getEigenValues().coeff(0) * scale;
  marker.pose.orientation.x = qx.x();
  marker.pose.orientation.y = qx.y();
  marker.pose.orientation.z = qx.z();
  marker.pose.orientation.w = qx.w();
  marker.color.b = 0.0;
  marker.color.g = 0.0;
  marker.color.r = 1.0;
  marker.color.a = 1.0;
  marker_array.markers.push_back(marker);

  marker.id = marker_id++;
  marker.scale.x = pca.getEigenValues().coeff(1) * scale;
  marker.pose.orientation.x = qy.x();
  marker.pose.orientation.y = qy.y();
  marker.pose.orientation.z = qy.z();
  marker.pose.orientation.w = qy.w();
  marker.color.b = 0.0;
  marker.color.g = 1.0;
  marker.color.r = 0.0;
  marker_array.markers.push_back(marker);

  marker.id = marker_id++;
  marker.scale.x = pca.getEigenValues().coeff(2) * scale;
  marker.pose.orientation.x = qz.x();
  marker.pose.orientation.y = qz.y();
  marker.pose.orientation.z = qz.z();
  marker.pose.orientation.w = qz.w();
  marker.color.b = 1.0;
  marker.color.g = 0.0;
  marker.color.r = 0.0;
  marker_array.markers.push_back(marker);
}
 bool compPCA(pcl::PCA<PointCloud::PointType> &pca, const PointCloud &pc, const float w, const Eigen::Vector2i &o, const Eigen::Vector2f &d) {
   PointCloud tmp;
   for(int x=0; x<w; x++) {
     Eigen::Vector2i p = o + (d*x).cast<int>();
     if(pcl_isfinite(pc(p(0),p(1)).getVector3fMap().sum()))
       tmp.push_back( pc(p(0),p(1)) );
   }
   if(tmp.size()<2) {
     ROS_WARN("no valid points");
     return false;
   }
   pca.compute(tmp);
   return true;
 }
예제 #7
0
파일: test_pca.cpp 프로젝트: nttputus/PCL
TEST(PCA, copy_constructor)
{
  // Test copy constructor
  pcl::PCA<pcl::PointXYZ> pca_copy(pca);
  try
  {
    Eigen::Matrix3f eigen_vectors_copy = pca_copy.getEigenVectors ();
    Eigen::Matrix3f eigen_vectors = pca.getEigenVectors ();
    for(size_t i = 0; i < 3; ++i)
      for(size_t j = 0; j < 3; ++j)
        EXPECT_EQ (eigen_vectors (i,j), eigen_vectors_copy (i,j));
  }
  catch (pcl::InitFailedException &e)
  {
    std::cerr << "something wrong" << std::endl;
  }
}