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);
}
示例#2
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;
  }
}