void FeatureCalculate::pca(CloudPtr cloud,PlanSegment &plane)
{    
    Eigen::MatrixXf X(3,cloud->size());
    double avr_x=0.0,avr_y=0.0,avr_z=0.0;
    for (int i = 0; i <cloud->size(); i++) {
        avr_x = avr_x  * double(i) / (i + 1) + cloud->points[i].x / double(i + 1);
        avr_y = avr_y  * double(i) / (i + 1) + cloud->points[i].y / double(i + 1);
        avr_z = avr_z  * double(i) / (i + 1) + cloud->points[i].z / double(i + 1);
    }
    for (int i=0;i<cloud->size();i++) 
    {
        X(0,i)=cloud->points[i].x-avr_x;
        X(1,i)=cloud->points[i].y-avr_y;
        X(2,i)=cloud->points[i].z-avr_z;
    }
    Eigen::MatrixXf XT(cloud->size(),3); 
    XT=X.transpose();
    Eigen::Matrix3f XXT; 
    XXT=X*XT;
    EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
    EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
    pcl::eigen33 (XXT, eigen_value, eigen_vector);
    plane.normal.normal_x = eigen_vector [0];
    plane.normal.normal_y = eigen_vector [1];
    plane.normal.normal_z = eigen_vector [2];
    plane.distance = - (plane.normal.normal_x * avr_x + plane.normal.normal_y * avr_y + plane.normal.normal_z * avr_z);

    float eig_sum = XXT.coeff (0) + XXT.coeff (4) + XXT.coeff (8);
    plane.min_value=eigen_value;
    if (eig_sum != 0)
    plane.normal.curvature = fabsf (eigen_value / eig_sum);
    else
        plane.normal.curvature = 0.0;
}
Beispiel #2
0
inline void
pcl::solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,
                           float &nx, float &ny, float &nz, float &curvature)
{
    // Avoid getting hung on Eigen's optimizers
//  for (int i = 0; i < 9; ++i)
//    if (!pcl_isfinite (covariance_matrix.coeff (i)))
//    {
//      //PCL_WARN ("[pcl::solvePlaneParameteres] Covariance matrix has NaN/Inf values!\n");
//      nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN ();
//      return;
//    }
    // Extract the smallest eigenvalue and its eigenvector
    EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
    EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
    pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);

    nx = eigen_vector [0];
    ny = eigen_vector [1];
    nz = eigen_vector [2];

    // Compute the curvature surface change
    float eig_sum = covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8);
    if (eig_sum != 0)
        curvature = fabsf (eigen_value / eig_sum);
    else
        curvature = 0;
}
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 checkCloud(const sensor_msgs::PointCloud2& cloud_msg,
                typename pcl::PointCloud<T>::Ptr hand_cloud,
                //typename pcl::PointCloud<T>::Ptr finger_cloud,
                const std::string& frame_id,
                tf::Vector3& hand_position,
                tf::Vector3& arm_position,
                tf::Vector3& arm_direction)
{  
  typename pcl::PointCloud<T>::Ptr cloud(new pcl::PointCloud<T>);
  pcl::fromROSMsg(cloud_msg, *cloud);

  if((cloud->points.size() < g_config.min_cluster_size) ||
     (cloud->points.size() > g_config.max_cluster_size))
    return false;

  pcl::PCA<T> pca;
  pca.setInputCloud(cloud);
  Eigen::Vector4f mean = pca.getMean();

  if((mean.coeff(0) < g_config.min_x) || (mean.coeff(0) > g_config.max_x)) return false;
  if((mean.coeff(1) < g_config.min_y) || (mean.coeff(1) > g_config.max_y)) return false;
  if((mean.coeff(2) < g_config.min_z) || (mean.coeff(2) > g_config.max_z)) return false;

  Eigen::Vector3f eigen_value = pca.getEigenValues();
  double ratio = eigen_value.coeff(0) / eigen_value.coeff(1);

  if((ratio < g_config.min_eigen_value_ratio) || (ratio > g_config.max_eigen_value_ratio)) return false;

  T search_point;
  Eigen::Matrix3f ev = pca.getEigenVectors();
  Eigen::Vector3f main_axis(ev.coeff(0, 0), ev.coeff(1, 0), ev.coeff(2, 0));
  main_axis.normalize();
  arm_direction.setX(main_axis.coeff(0));
  arm_direction.setY(main_axis.coeff(1));
  arm_direction.setZ(main_axis.coeff(2));
  arm_position.setX(mean.coeff(0));
  arm_position.setY(mean.coeff(1));
  arm_position.setZ(mean.coeff(2));

  main_axis = (-main_axis * 0.3) + Eigen::Vector3f(mean.coeff(0), mean.coeff(1), mean.coeff(2));
  search_point.x = main_axis.coeff(0);
  search_point.y = main_axis.coeff(1);
  search_point.z = main_axis.coeff(2);



  //find hand
  pcl::KdTreeFLANN<T> kdtree;
  kdtree.setInputCloud(cloud);

  //find the closet point from the serach_point
  std::vector<int> point_indeices(1);
  std::vector<float> point_distances(1);
  if ( kdtree.nearestKSearch (search_point, 1, point_indeices, point_distances) > 0 )
  {
    //update search point
    search_point = cloud->points[point_indeices[0]];

    //show seach point
    if(g_marker_array_pub.getNumSubscribers() != 0)
    {
      pushSimpleMarker(search_point.x, search_point.y, search_point.z,
                       1.0, 0, 0,
                       0.02,
                       g_marker_id, g_marker_array, frame_id);
    }

    //hand
    point_indeices.clear();
    point_distances.clear();
    kdtree.radiusSearch(search_point, g_config.hand_lenght, point_indeices, point_distances);
    for (size_t i = 0; i < point_indeices.size (); ++i)
    {
      hand_cloud->points.push_back(cloud->points[point_indeices[i]]);
      hand_cloud->points.back().r = 255;
      hand_cloud->points.back().g = 0;
      hand_cloud->points.back().b = 0;
    }

    Eigen::Vector4f centroid;
    pcl::compute3DCentroid(*hand_cloud, centroid);

    if(g_marker_array_pub.getNumSubscribers() != 0)
    {
      pushSimpleMarker(centroid.coeff(0), centroid.coeff(1), centroid.coeff(2),
                       0.0, 1.0, 0,
                       0.02,
                       g_marker_id, g_marker_array, frame_id);
    }

    hand_position.setX(centroid.coeff(0));
    hand_position.setY(centroid.coeff(1));
    hand_position.setZ(centroid.coeff(2));

#if 0
    //fingers
    search_point.x = centroid.coeff(0);
    search_point.y = centroid.coeff(1);
    search_point.z = centroid.coeff(2);
    std::vector<int> point_indeices_inner;
    std::vector<float> point_distances_inner;
    kdtree.radiusSearch(search_point, 0.07, point_indeices_inner, point_distances_inner);

    std::vector<int> point_indeices_outter;
    std::vector<float> point_distances_outter;
    kdtree.radiusSearch(search_point, 0.1, point_indeices_outter, point_distances_outter);

    //ROS_INFO("before %d %d", point_indeices_inner.size(), point_indeices_outter.size());

    std::vector<int>::iterator it;
    for(size_t i = 0; i < point_indeices_inner.size(); i++)
    {

      it =  std::find(point_indeices_outter.begin(), point_indeices_outter.end(), point_indeices_inner[i]);
      if(it != point_indeices_outter.end())
      {
        point_indeices_outter.erase(it);
      }
    }

    //ROS_INFO("after %d %d", point_indeices_inner.size(), point_indeices_outter.size());

    //ROS_DEBUG_THROTTLE(1.0, "found %lu", point_indeices.size ());
    for (size_t i = 0; i < point_indeices_outter.size (); ++i)
    {
      finger_cloud->points.push_back(cloud->points[point_indeices_outter[i]]);
      finger_cloud->points.back().r = 255;
      finger_cloud->points.back().g = 0;
      finger_cloud->points.back().b = 0;
    }
#endif

  }
  else
  {    
    return false;
  }

  if(g_marker_array_pub.getNumSubscribers() != 0)
  {
    pushEigenMarker<T>(pca, g_marker_id, g_marker_array, 0.1, frame_id);
  }


  return true;
}
void 
pcl::getCameraMatrixFromProjectionMatrix (
    const Eigen::Matrix<float, 3, 4, Eigen::RowMajor>& projection_matrix, 
    Eigen::Matrix3f& camera_matrix)
{
  Eigen::Matrix3f KR = projection_matrix.topLeftCorner <3, 3> ();

  Eigen::Matrix3f KR_KRT = KR * KR.transpose ();
  
  Eigen::Matrix3f cam = KR_KRT / KR_KRT.coeff (8);

  memset (&(camera_matrix.coeffRef (0)), 0, sizeof (Eigen::Matrix3f::Scalar) * 9);
  camera_matrix.coeffRef (8) = 1.0;
  
  if (camera_matrix.Flags & Eigen::RowMajorBit)
  {
    camera_matrix.coeffRef (2) = cam.coeff (2);
    camera_matrix.coeffRef (5) = cam.coeff (5);
    camera_matrix.coeffRef (4) = static_cast<float> (std::sqrt (cam.coeff (4) - cam.coeff (5) * cam.coeff (5)));
    camera_matrix.coeffRef (1) = (cam.coeff (1) - cam.coeff (2) * cam.coeff (5)) / camera_matrix.coeff (4);
    camera_matrix.coeffRef (0) = static_cast<float> (std::sqrt (cam.coeff (0) - camera_matrix.coeff (1) * camera_matrix.coeff (1) - cam.coeff (2) * cam.coeff (2)));
  }
  else
  {
    camera_matrix.coeffRef (6) = cam.coeff (2);
    camera_matrix.coeffRef (7) = cam.coeff (5);
    camera_matrix.coeffRef (4) = static_cast<float> (std::sqrt (cam.coeff (4) - cam.coeff (5) * cam.coeff (5)));
    camera_matrix.coeffRef (3) = (cam.coeff (1) - cam.coeff (2) * cam.coeff (5)) / camera_matrix.coeff (4);
    camera_matrix.coeffRef (0) = static_cast<float> (std::sqrt (cam.coeff (0) - camera_matrix.coeff (3) * camera_matrix.coeff (3) - cam.coeff (2) * cam.coeff (2)));
  }
}
Beispiel #6
0
template <typename PointT> inline unsigned int
pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
                                const std::vector<int> &indices,
                                Eigen::Matrix3f &covariance_matrix,
                                Eigen::Vector4f &centroid)
{
  // create the buffer on the stack which is much faster than using cloud.points[indices[i]] and centroid as a buffer
  Eigen::Matrix<float, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<float, 1, 9, Eigen::RowMajor>::Zero ();
  size_t point_count;
  if (cloud.is_dense)
  {
    point_count = indices.size ();
    for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
    {
      //const PointT& point = cloud[*iIt];
      accu [0] += cloud[*iIt].x * cloud[*iIt].x;
      accu [1] += cloud[*iIt].x * cloud[*iIt].y;
      accu [2] += cloud[*iIt].x * cloud[*iIt].z;
      accu [3] += cloud[*iIt].y * cloud[*iIt].y;
      accu [4] += cloud[*iIt].y * cloud[*iIt].z;
      accu [5] += cloud[*iIt].z * cloud[*iIt].z;
      accu [6] += cloud[*iIt].x;
      accu [7] += cloud[*iIt].y;
      accu [8] += cloud[*iIt].z;
    }
  }
  else
  {
    point_count = 0;
    for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
    {
      if (!isFinite (cloud[*iIt]))
        continue;

      ++point_count;
      accu [0] += cloud[*iIt].x * cloud[*iIt].x;
      accu [1] += cloud[*iIt].x * cloud[*iIt].y;
      accu [2] += cloud[*iIt].x * cloud[*iIt].z;
      accu [3] += cloud[*iIt].y * cloud[*iIt].y; // 4
      accu [4] += cloud[*iIt].y * cloud[*iIt].z; // 5
      accu [5] += cloud[*iIt].z * cloud[*iIt].z; // 8
      accu [6] += cloud[*iIt].x;
      accu [7] += cloud[*iIt].y;
      accu [8] += cloud[*iIt].z;
    }
  }

  accu /= static_cast<float> (point_count);
  //Eigen::Vector3f vec = accu.tail<3> ();
  //centroid.head<3> () = vec;//= accu.tail<3> ();
  //centroid.head<3> () = accu.tail<3> ();    -- does not compile with Clang 3.0
  centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8];
  centroid[3] = 0;
  covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
  covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
  covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
  covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
  covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
  covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
  covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
  covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
  covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);

  return (static_cast<unsigned int> (point_count));
}
Beispiel #7
0
template <typename PointT> inline unsigned int
pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
                                     Eigen::Matrix3f &covariance_matrix,
                                     Eigen::Vector4f &centroid)
{
  // create the buffer on the stack which is much faster than using cloud.points[indices[i]] and centroid as a buffer
  Eigen::Matrix<float, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<float, 1, 9, Eigen::RowMajor>::Zero ();
  unsigned point_count;
  if (cloud.is_dense)
  {
    point_count = cloud.size ();
    // For each point in the cloud
    for (size_t i = 0; i < point_count; ++i)
    {
      accu [0] += cloud[i].x * cloud[i].x;
      accu [1] += cloud[i].x * cloud[i].y;
      accu [2] += cloud[i].x * cloud[i].z;
      accu [3] += cloud[i].y * cloud[i].y; // 4
      accu [4] += cloud[i].y * cloud[i].z; // 5
      accu [5] += cloud[i].z * cloud[i].z; // 8
      accu [6] += cloud[i].x;
      accu [7] += cloud[i].y;
      accu [8] += cloud[i].z;
    }
  }
  else
  {
    point_count = 0;
    for (size_t i = 0; i < cloud.points.size (); ++i)
    {
      if (!isFinite (cloud[i]))
        continue;

      accu [0] += cloud[i].x * cloud[i].x;
      accu [1] += cloud[i].x * cloud[i].y;
      accu [2] += cloud[i].x * cloud[i].z;
      accu [3] += cloud[i].y * cloud[i].y;
      accu [4] += cloud[i].y * cloud[i].z;
      accu [5] += cloud[i].z * cloud[i].z;
      accu [6] += cloud[i].x;
      accu [7] += cloud[i].y;
      accu [8] += cloud[i].z;
      ++point_count;
    }
  }
  accu /= (float) point_count;
  if (point_count != 0)
  {
    centroid.head<3> () = accu.tail<3> ();
    centroid[3] = 0;
    covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
    covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
    covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
    covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
    covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
    covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
    covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
    covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
    covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
  }
  return (point_count);
}