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; }