From 076f9bfcea6721701b978170801de4c9acbbbbf0 Mon Sep 17 00:00:00 2001 From: TescaF Date: Tue, 1 Aug 2017 10:19:40 -0400 Subject: [PATCH 01/28] updated feature extraction for new hlpr_perception messages --- hlpr_feature_extraction/CMakeLists.txt | 21 +- .../include/objectFeatures.hpp | 421 ++++++++++++++---- .../include/utils_pcl_ros.hpp | 25 +- .../msg/PcFeatureArray.msg | 5 - hlpr_feature_extraction/msg/PcFeatures.msg | 36 -- hlpr_feature_extraction/package.xml | 2 +- .../src/cluster_processing.cpp | 2 +- .../src/feature_extraction.cpp | 18 +- hlpr_feature_extraction/src/utils_pcl_ros.cpp | 113 ++++- 9 files changed, 483 insertions(+), 160 deletions(-) delete mode 100644 hlpr_feature_extraction/msg/PcFeatureArray.msg delete mode 100644 hlpr_feature_extraction/msg/PcFeatures.msg diff --git a/hlpr_feature_extraction/CMakeLists.txt b/hlpr_feature_extraction/CMakeLists.txt index cc054c0..d0213fa 100644 --- a/hlpr_feature_extraction/CMakeLists.txt +++ b/hlpr_feature_extraction/CMakeLists.txt @@ -17,31 +17,18 @@ set(ROS_LIBRARIES /opt/ros/${ROS_DISTRO}/lib/libroscpp.so #find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure message_generation roscpp std_msgs geometry_msgs) -find_package(catkin REQUIRED COMPONENTS message_generation roscpp std_msgs geometry_msgs tf2_ros hlpr_segmentation) +find_package(catkin REQUIRED COMPONENTS message_generation roscpp std_msgs geometry_msgs tf2_ros hlpr_perception_msgs) find_package(PCL 1.8 REQUIRED COMPONENTS) find_package(freenect2 REQUIRED) find_package(OpenCV 2 REQUIRED) -add_message_files( - FILES - PcFeatures.msg - PcFeatureArray.msg -) - -generate_messages( - DEPENDENCIES - std_msgs - geometry_msgs - hlpr_segmentation -) - #generate_dynamic_reconfigure_options( # cfg/segmentation.cfg #) catkin_package( # CATKIN_DEPENDS dynamic_reconfigure message_runtime roscpp std_msgs geometry_msgs - CATKIN_DEPENDS message_runtime roscpp std_msgs geometry_msgs hlpr_segmentation + CATKIN_DEPENDS message_runtime roscpp std_msgs geometry_msgs hlpr_perception_msgs ) include_directories(${PCL_INCLUDE_DIRS}) @@ -54,13 +41,13 @@ include_directories(${PROJECT_SOURCE_DIR}/include) add_library(ftex SHARED src/cluster_processing.cpp src/rotcalipers.cpp src/utils.cpp src/utils_pcl_ros.cpp) target_link_libraries(ftex ${PCL_LIBRARIES} ${ROS_LIBRARIES}) -add_dependencies(ftex hlpr_feature_extraction_generate_messages_cpp) +add_dependencies(ftex hlpr_perception_msgs_generate_messages_cpp) add_executable(ft_ex src/feature_extraction.cpp) #add_executable(listener src/listener.cpp src/nodes/listener_node.cpp) #target_link_libraries(pc_seg ${catkin_LIBRARIES} ${freenect2_LIBRARY} ${ROS_LIBRARIES} ${OpenCV_LIBS}) target_link_libraries(ft_ex ${PCL_LIBRARIES} ftex ${freenect2_LIBRARY} ${ROS_LIBRARIES} ${OpenCV_LIBS}) -add_dependencies(ft_ex hlpr_feature_extraction_generate_messages_cpp) +add_dependencies(ft_ex hlpr_perception_msgs_generate_messages_cpp) #add_dependencies(pc_seg segmentation_gencfg segmentation_generate_messages_cpp) diff --git a/hlpr_feature_extraction/include/objectFeatures.hpp b/hlpr_feature_extraction/include/objectFeatures.hpp index 6e63c08..75a342f 100644 --- a/hlpr_feature_extraction/include/objectFeatures.hpp +++ b/hlpr_feature_extraction/include/objectFeatures.hpp @@ -8,6 +8,8 @@ #define OBJECTFEATURES_HPP_ #include +#include +#include #include #include @@ -22,10 +24,10 @@ #include #include #include +#include //use eigen data structures? - class pc_cluster_features { private: @@ -36,46 +38,67 @@ class pc_cluster_features } - void boundingBoxWithZ(pcl::PointCloud &cluster) + // Added by Priyanka - for non-planar segmentation + void orientedBoundingBox(pcl::PointCloud &cluster) { - pcl::PointCloud::Ptr projected_cluster = cluster.makeShared(); - for(size_t j = 0; j < projected_cluster->points.size(); j++) - projected_cluster->points[j].z = 1; - - volume2 = cluster.size(); - - pcl::getMinMax3D(cluster, min, max); - aligned_bounding_box = minAreaRect(projected_cluster); - aligned_bounding_box.center.z = min[2]+(max[2]-min[2])/2; //this should be maxheight/2+planeHeight - aligned_bounding_box.size.zSize = fabs((float)(max[2]-min[2])); //this should be max height! - - aligned_bounding_box.fillQuatGivenAxisAngle(); - } + pcl::PointCloud::Ptr projected_cluster = cluster.makeShared(); + volume2 = cluster.size(); + pcl::getMinMax3D(cluster, min, max); + + std::vector moment_of_inertia; + std::vector eccentricity; + pcl::PointXYZ min_point_OBB; + pcl::PointXYZ max_point_OBB; + pcl::PointXYZ position_OBB; + Eigen::Matrix3f rotational_matrix_OBB; + //Eigen::Vector3f major_vector, middle_vector, minor_vector; + + pcl::PointCloud::Ptr cluster_xyzrgb (projected_cluster); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + pcl::copyPointCloud(*cluster_xyzrgb, *cloud); + pcl::MomentOfInertiaEstimation feature_extractor; + feature_extractor.setInputCloud(cloud); + feature_extractor.compute(); + + //feature_extractor.getMomentOfInertia (moment_of_inertia); + //feature_extractor.getEccentricity (eccentricity); + feature_extractor.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB); + //feature_extractor.getEigenValues(major_value, middle_value, minor_value); + //feature_extractor.getEigenVectors(major_vector, middle_vector, minor_vector); + //feature_extractor.getMassCenter(mass_center); + + // Fill up the Box3D structure + Eigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z); + oriented_bounding_box.center.x = position(0); + oriented_bounding_box.center.y = position(1); + oriented_bounding_box.center.z = position(2); + + std::cout<<"Center of OBB: ("<< position(0) <<", "< quat (rotational_matrix_OBB); + oriented_bounding_box.rot_quat[0] = quat.x(); + oriented_bounding_box.rot_quat[1] = quat.y(); + oriented_bounding_box.rot_quat[2] = quat.z(); + oriented_bounding_box.rot_quat[3] = quat.w(); + + oriented_bounding_box.size.xSize = max_point_OBB.x - min_point_OBB.x; + std::cout<<"xSize : "< &cluster, pcl::ModelCoefficients::Ptr coefficients) { pcl::PointCloud::Ptr projected_cluster = cluster.makeShared(); - pcl::PointCloud::Ptr cloud_transformed (new pcl::PointCloud); - /*pcl::PointCloud::Ptr cloud_projected (new pcl::PointCloud); - pcl::ProjectInliers proj; - proj.setModelType (pcl::SACMODEL_PLANE); - proj.setInputCloud (cluster.makeShared()); - proj.setModelCoefficients (coefficients); - proj.filter (*cloud_projected);*/ - Eigen::Vector3f z_axis(0,0,1); Eigen::Vector3f plane_normal(coefficients->values[0],coefficients->values[1],coefficients->values[2]); -// Eigen::Vector3f plane_normal(0.0270649, 0.849503, 0.526889); - plane_normal.normalize(); - Eigen::Vector3f c = plane_normal.cross(z_axis);//z_axis.cross(plane_normal); - c.normalize(); - - //std::cout << c << std::endl; - //std::cout << plane_normal << std::endl << std::endl; + c.normalize(); double cost = plane_normal.dot(z_axis); double half_theta = acos(cost)/2; @@ -86,55 +109,64 @@ class pc_cluster_features Eigen::Affine3f T = Eigen::Affine3f::Identity(); T.rotate(R); - //std::cout << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " " << std::endl; - pcl::transformPointCloud (*projected_cluster, *cloud_transformed, T); volume2 = cluster.size(); pcl::getMinMax3D(*cloud_transformed, min, max); for(size_t j = 0; j < cloud_transformed->points.size(); j++) - cloud_transformed->points[j].z = 1; - - aligned_bounding_box = minAreaRect(cloud_transformed); + cloud_transformed->points[j].z = 1; - - Eigen::Quaternion q2(cos(-aligned_bounding_box.angle/2), 0, 0, sin(-aligned_bounding_box.angle/2)); + oriented_bounding_box = minAreaRect(cloud_transformed); + Eigen::Quaternion q2(cos(-oriented_bounding_box.angle/2), 0, 0, sin(-oriented_bounding_box.angle/2)); Eigen::Quaternion q3 = q2*q; q3 = q3.inverse(); - //pcl::getMinMax3D(cluster, min, max); - //aligned_bounding_box = minAreaRect(projected_cluster); - - /*for(int i = 0; i< 3 ; i++) - aligned_bounding_box.rot_axis[i] = c[i]; - aligned_bounding_box.fillQuatGivenAxisAngle();*/ - - aligned_bounding_box.center.z = min[2]+(max[2]-min[2])/2; //this should be maxheight/2+planeHeight - aligned_bounding_box.size.zSize = fabs((float)(max[2]-min[2])); //this should be max height! + oriented_bounding_box.center.z = min[2]+(max[2]-min[2])/2; //this should be maxheight/2+planeHeight + oriented_bounding_box.size.zSize = fabs((float)(max[2]-min[2])); //this should be max height! - Eigen::Vector3f X(aligned_bounding_box.center.x,aligned_bounding_box.center.y,aligned_bounding_box.center.z); + Eigen::Vector3f X(oriented_bounding_box.center.x,oriented_bounding_box.center.y,oriented_bounding_box.center.z); Eigen::Vector3f Y(0,0,0); Y = T.inverse()*X; - aligned_bounding_box.center.x = Y.x(); - aligned_bounding_box.center.y = Y.y(); - aligned_bounding_box.center.z = Y.z(); + oriented_bounding_box.center.x = Y.x(); + oriented_bounding_box.center.y = Y.y(); + oriented_bounding_box.center.z = Y.z(); + oriented_bounding_box.rot_quat[0] = q3.x(); + oriented_bounding_box.rot_quat[1] = q3.y(); + oriented_bounding_box.rot_quat[2] = q3.z(); + oriented_bounding_box.rot_quat[3] = q3.w(); + } + + // Added by Priyanka - for planes + void planeConst(pcl::PointCloud &contour) + { + plane_cloud = contour.makeShared(); - /*for(int i = 0; i < 3; i++) - aligned_bounding_box.rot_axis[i] = c[i];//coefficients->values[i]; + float r = 0.0f; + float g = 0.0f; + float b = 0.0f; + float n = static_cast (contour.size()); + for (size_t j = 0; j < n; j++) + { + r += (float) (contour[j].r); + g += (float) (contour[j].g); + b += (float) (contour[j].b); + } + r /= n; g /= n; b /= n; - aligned_bounding_box.angle = //half_theta*2; + hue = rgb2hue(r,g,b); - aligned_bounding_box.fillQuatGivenAxisAngle();*/ + //std::cout << rgb2hue(r,g,b) << endl; - aligned_bounding_box.rot_quat[0] = q3.x(); - aligned_bounding_box.rot_quat[1] = q3.y(); - aligned_bounding_box.rot_quat[2] = q3.z(); - aligned_bounding_box.rot_quat[3] = q3.w(); + int ri = r; int gi = (int)g; int bi = (int)b; + color[0] = (unsigned char) ri; + color[1] = (unsigned char) gi; + color[2] = (unsigned char) bi; + fill(); } void clusterConst(pcl::PointCloud &cluster) @@ -186,7 +218,9 @@ class pc_cluster_features pcl::compute3DCentroid(cluster, centroid); - boundingBoxWithZ(cluster); + orientedBoundingBox(cluster); // Changed by Priyanka for non-planar segmentation + + //boundingBoxWithZ(cluster); //BARIS: PROJECT HERE // pcl::transformPointCloud (cluster, *projected_cluster, plane_transformation); @@ -202,8 +236,8 @@ class pc_cluster_features // projected_cluster->points[j].z = 1; clusterConst(cluster); - } + pc_cluster_features(pcl::PointCloud &cluster, Eigen::Vector4f &used_plane_model) { defaultConst(); @@ -225,18 +259,31 @@ class pc_cluster_features if(zero_count > 3) use_projection = false; - if(use_projection) + //if(use_projection) boundingBoxWithCoeff(cluster, coefficients); - else - boundingBoxWithZ(cluster); + //else + // boundingBoxWithZ(cluster); clusterConst(cluster); } + pc_cluster_features(pcl::PointCloud::Ptr &contour){ + defaultConst(); + + pcl::compute3DCentroid(*contour, centroid); + + orientedBoundingBox(*contour); + + planeConst(*contour); + + orientedBoundingBox(*contour); + } + ~pc_cluster_features(){} - Box3D aligned_bounding_box; + //Box3D aligned_bounding_box; + Box3D oriented_bounding_box; //Box3D aligned_bounding_box2; //FEATURES @@ -248,7 +295,7 @@ class pc_cluster_features //Eigen::Vector3f color; ColorVec color; float hue; - + Eigen::Vector4f centroid; Eigen::Quaternionf rotation_quat; // kind of hard to get full orientation, need to fit a box which is not trivial! n^3, needs 3d convez hull and hard to implement, approximations exist @@ -257,7 +304,7 @@ class pc_cluster_features //assuming on table, coming from the bounding box float bb_aspect_ratio; - float bb_orientation; //wrt to the table, use a complex number? + //float bb_orientation; //wrt to the table, use a complex number? float compactness; // volume/bb_volume need a better name? float av_ratio; // area/volume @@ -268,24 +315,46 @@ class pc_cluster_features float objectBuffer[5000]; pcl::PointCloud::Ptr cloud; + pcl::PointCloud::Ptr plane_cloud; pcl::PointCloud::Ptr parent_cloud; pcl::PointIndices indices; //indices in the parent point cloud pcl::PointCloud normals; + + bool setViewpointHist = true; // boolean to check if viewpoint Histogram is being computed and set (Default - true) pcl::PointCloud vfhs;// (new pcl::PointCloud ()); - + + bool setShapeHist = true; // boolean to check if shape histogram is being computed and set (Default - true) + pcl::PointCloud cvfhs; // (new pcl::PointCloud ()); + pcl::PointCloud fpfhs; //(new pcl::PointCloud ()); + + // Normalized cvfh feature vector? + //std::vector histogram_cvfh_vector (308); + + // Normalized fpfh feature vector? + //std::vector histogram_fpfh_vector (308); + + // Normalized hue-saturation histogram + bool setColorHist = true; // boolean to check if color Histogram is being computed and set (Default - true) + std::vector histogram_hs; + + // Other features (default is set to false) + bool setOtherFeatures = false; + std::vector otherFeatures; // Vector to store the other features + //Image based features??? blob features, color histogram? using opencv void fill() { - aligned_bounding_box.calculateProperties(); + oriented_bounding_box.calculateProperties(); //aligned_bounding_box.fillQuatGivenAxisAngle(); - bb_orientation = aligned_bounding_box.angle; - bb_volume = aligned_bounding_box.volume; - bb_area = aligned_bounding_box.area; - bb_aspect_ratio = aligned_bounding_box.aspect_ratio; - rotation_quat = Eigen::Quaternionf(aligned_bounding_box.rot_quat); - + //bb_orientation = aligned_bounding_box.angle; + bb_volume = oriented_bounding_box.volume; + bb_area = oriented_bounding_box.area; + bb_aspect_ratio = oriented_bounding_box.aspect_ratio; + rotation_quat = Eigen::Quaternionf(oriented_bounding_box.rot_quat); + + //NOTE: compactness is not valid for planes compactness = (float)bb_volume/volume2; //where is the real area/real volume? av_ratio = bb_area/bb_volume; @@ -299,10 +368,10 @@ class pc_cluster_features out_features[feature_counter++] = numFeatures; - out_features[feature_counter++] = aligned_bounding_box.center.x; - out_features[feature_counter++] = aligned_bounding_box.center.y; - out_features[feature_counter++] = aligned_bounding_box.center.z; - out_features[feature_counter++] = aligned_bounding_box.angle; + out_features[feature_counter++] = oriented_bounding_box.center.x; + out_features[feature_counter++] = oriented_bounding_box.center.y; + out_features[feature_counter++] = oriented_bounding_box.center.z; + //out_features[feature_counter++] = oriented_bounding_box.angle; for(int i = 0;i<3;feature_counter++,i++) out_features[feature_counter] = centroid[i]; @@ -320,9 +389,9 @@ class pc_cluster_features out_features[feature_counter++] = volume2; - out_features[feature_counter++] = aligned_bounding_box.size.xSize; - out_features[feature_counter++] = aligned_bounding_box.size.ySize; - out_features[feature_counter++] = aligned_bounding_box.size.zSize; + out_features[feature_counter++] = oriented_bounding_box.size.xSize; + out_features[feature_counter++] = oriented_bounding_box.size.ySize; + out_features[feature_counter++] = oriented_bounding_box.size.zSize; out_features[feature_counter++] = bb_volume; out_features[feature_counter++] = bb_area; @@ -400,5 +469,195 @@ for(size_t j = 0; j < projected_cluster->points.size(); j++) projected_cluster->points[j].z = projected_cluster->points[j].z - coefficients->values[2]*t; }*/ +typedef pcl::PointCloud PointCloudT; +typedef unsigned int uint; + +// Histogram of hue and saturation +class HSHistogram{ + private: + int dim; + public: + HSHistogram(int dim):dim(dim){ } // empty constructor -> just sets the dimension of the histogram + + void computeHistogram(PointCloudT &cloud, std::vector > &hist2, std::vector &hist2_double_vector){ + int cloud_size = cloud.points.size(); + + for(int i = 0; i < cloud_size; i++){ + int r = (int)(double)(cloud.points[i].r); + int g = (int)(double)(cloud.points[i].g); + int b = (int)(double)(cloud.points[i].b); + + // convert the rgb value to HS value + std::pair HS = rgb2hue(r,g,b); + //std::cout<<"Value of hue: "<(HS)<< " sat: "<(HS)<(HS) / round); // Get a bin for hue from 0-15 + int s = (int) (std::get<1>(HS) * (dim-1)); // Get a bin for saturation from 0-15 + + //std::cout<<"Value of h: "< rgb2hue(int r, int g, int b){ + const unsigned char max = std::max (r, std::max (g, b)); + const unsigned char min = std::min (r, std::min (g, b)); + + float hue; + float sat; + + const float diff = static_cast (max - min); + + if (max == 0) // division by zero + { + sat = 0; + //hue undefined! set to your favorite value (any value with zero saturation will produce black) + hue = 0; + } + else + { + sat = diff/max; + + if (min == max) // diff == 0 -> division by zero + { + sat = 0; + //hue undefined! set to your favorite value (black) + hue = 0; + } + else + { + if (max == r) hue = 60.f * ( static_cast (g - b) / diff); + else if (max == g) hue = 60.f * (2.f + static_cast (b - r) / diff); + else hue = 60.f * (4.f + static_cast (r - g) / diff); // max == b + + if (hue < 0.f) hue += 360.f; + } + } + + //if (sat < ridiculous_global_variables::saturation_threshold && ridiculous_global_variables::ignore_low_sat) + //hue = ridiculous_global_variables::saturation_mapped_value; //hackzz oh the hackz + + // Return both hue and saturation as a pair + std::pair returnHS = std::make_pair(hue,sat); + + return returnHS; + } +}; + +// Helper class to help with colour histograms +/*class ColorHistogram { + private: + //std::vector > > hist3; + int dim; + public: + /*ColorHistogram(int dim):dim(dim) { + cloud_size = 0; + hist3.resize(dim); + for (int i = 0; i < dim; i++) { + hist3[i].resize(dim); + for (int j = 0; j < dim; j++) { + hist3[i][j].resize(dim); + std::fill( hist3[i][j].begin(), hist3[i][j].end(), 0 ); + } + } + }*/ + + //ColorHistogram(int dim):dim(dim){ } // empty constructor + + /*void computeHistogram(PointCloudT &cloud, std::vector > > &hist3, std::vector &hist3_double_vector) { + //ROS_INFO("Computing color histogram..."); + int cloud_size = cloud.points.size(); + for (int i = 0; i < cloud_size; i++) { + // Max value of 255. We want 256 because we want the rgb division to result in + // a value always slightly smaller than the dim due to index starting from 0 + double round = (double)256 / dim; + int r = (int)((double)(cloud.points[i].r) / round); + int g = (int)((double)(cloud.points[i].g) / round); + int b = (int)((double)(cloud.points[i].b) / round); + + //std::cout<<"\nValue before updating: "< toDoubleVector() { + int i_offset = dim * dim; + int j_offset = dim; + std::vector hist3_double_vector (dim * dim * dim, 0); + for (int i = 0; i < dim; i++) { + for (int j = 0; j < dim; j++) { + for (int k = 0; k < dim; k++) { + hist3_double_vector[i * i_offset + j * j_offset + k] = hist3[i][j][k]; + std::cout <<"\nValue of hist3: "< toDoubleVectorNormalized() { + std::vector hist_double_vector = toDoubleVector(); + for (int i = 0; i < hist_double_vector.size(); i++) { + hist_double_vector[i] /= cloud_size; + } + return hist_double_vector; + }*/ +//}; #endif /* OBJECTFEATURES_HPP_ */ diff --git a/hlpr_feature_extraction/include/utils_pcl_ros.hpp b/hlpr_feature_extraction/include/utils_pcl_ros.hpp index 3038867..531aab4 100644 --- a/hlpr_feature_extraction/include/utils_pcl_ros.hpp +++ b/hlpr_feature_extraction/include/utils_pcl_ros.hpp @@ -18,6 +18,11 @@ #include #include #include +#include +#include +#include +#include +#include #include /* @@ -43,8 +48,26 @@ Box3D //boundingBoxWithCoeff(pcl::PointCloud &cluster, pcl::ModelCoefficients::Ptr coefficients); boundingBoxWithCoeff(pcl::PointCloud &cluster, pcl::ModelCoefficients::Ptr coefficients, pcl::PointCloud::Ptr &cloud_transformed); +//void +//fillRosMessage (hlpr_feature_extraction::PcFeatures &outRosMsg, const pc_cluster_features &inObjFeatures); + +void +fillBasicFeaturesMsg (hlpr_perception_msgs::BasicFeatures &basicInfo, const pc_cluster_features &inObjFeatures); + +void +fillOrientedBoundingBoxMsg (hlpr_perception_msgs::OrientedBoundingBox &obb, const pc_cluster_features &inObjFeatures); + +void +fillRosMessageForObjects (hlpr_perception_msgs::ObjectFeatures &objRosMsg, + const pc_cluster_features &inObjFeatures); + +// Top-level message for Objects void -fillRosMessage (hlpr_feature_extraction::PcFeatures &outRosMsg, const pc_cluster_features &inObjFeatures); +fillObjectFeaturesMsg (hlpr_perception_msgs::ObjectFeatures &objRosMsg, + hlpr_perception_msgs::BasicFeatures &basicInfo, + hlpr_perception_msgs::OrientedBoundingBox &obb, + hlpr_perception_msgs::OtherFeatures &other, + const pc_cluster_features &inObjFeatures); void objectPoseTF(geometry_msgs::Transform geom_transform); diff --git a/hlpr_feature_extraction/msg/PcFeatureArray.msg b/hlpr_feature_extraction/msg/PcFeatureArray.msg deleted file mode 100644 index 02b6a9e..0000000 --- a/hlpr_feature_extraction/msg/PcFeatureArray.msg +++ /dev/null @@ -1,5 +0,0 @@ -# PcFeatures for a single cluster - -Header header -PcFeatures[] objects -geometry_msgs/Transform[] transforms diff --git a/hlpr_feature_extraction/msg/PcFeatures.msg b/hlpr_feature_extraction/msg/PcFeatures.msg deleted file mode 100644 index abfb251..0000000 --- a/hlpr_feature_extraction/msg/PcFeatures.msg +++ /dev/null @@ -1,36 +0,0 @@ -# PcFeatures for a single cluster - -Header header - -# Object transform, however calculated -geometry_msgs/Transform transform - - -################################# -# Raw point Related - -# Cluster centroid, min, max and number of points -geometry_msgs/Vector3 points_centroid -geometry_msgs/Vector3 points_min # -geometry_msgs/Vector3 points_max # -float64 num_points - -# Average color (RGBA nad hue) -std_msgs/ColorRGBA rgba_color -float64 hue - -################################# -#Bounding box - -#position wrt sensor and angle wrt table normal -geometry_msgs/Vector3 bb_center -float64 bb_angle - -# Bounding box dimensions -geometry_msgs/Vector3 bb_dims - -################################# -#Other such as vfh, color hist etc. Unpacking will be on the other side - -uint32 other_features_size -float64[] data \ No newline at end of file diff --git a/hlpr_feature_extraction/package.xml b/hlpr_feature_extraction/package.xml index 3fc5145..ae18cb6 100644 --- a/hlpr_feature_extraction/package.xml +++ b/hlpr_feature_extraction/package.xml @@ -15,5 +15,5 @@ rospy std_msgs geometry_msgs - hlpr_segmentation + hlpr_perception_msgs diff --git a/hlpr_feature_extraction/src/cluster_processing.cpp b/hlpr_feature_extraction/src/cluster_processing.cpp index e865bc2..d458202 100644 --- a/hlpr_feature_extraction/src/cluster_processing.cpp +++ b/hlpr_feature_extraction/src/cluster_processing.cpp @@ -312,7 +312,7 @@ void OpenNIOrganizedMultiPlaneSegmentation::featureExtraction( //Feature Extraction pc_cluster_features feat(cluster, used_plane_model); - fittedBoxes.push_back(feat.aligned_bounding_box); + fittedBoxes.push_back(feat.oriented_bounding_box); cluster_colors.push_back(feat.color); vfh.setInputCloud (cluster.makeShared()); diff --git a/hlpr_feature_extraction/src/feature_extraction.cpp b/hlpr_feature_extraction/src/feature_extraction.cpp index 07f305c..7769720 100644 --- a/hlpr_feature_extraction/src/feature_extraction.cpp +++ b/hlpr_feature_extraction/src/feature_extraction.cpp @@ -20,9 +20,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -108,7 +108,7 @@ cloud_cb_ros_ (const sensor_msgs::PointCloud2ConstPtr& msg) void -cluster_cb (const hlpr_segmentation::SegmentedClusters& msg) +cluster_cb (const hlpr_perception_msgs::SegClusters& msg) { clusters.clear(); for(int i = 0; i < msg.clusters.size(); i++) { @@ -202,7 +202,7 @@ main (int argc, char **argv) std::cout << "Publishing ros topic: " << outRostopic << std::endl; //pub = nh->advertise(outRostopic,5); - pub = nh->advertise(outRostopic,5); + pub = nh->advertise(outRostopic,5); objectPub = nh->advertise(objectPCOutRostopic,5); //transformPub = nh->advertise(transformRostopic,5); @@ -238,16 +238,16 @@ main (int argc, char **argv) if(selected_cluster_index < 0) continue; - float angle = feats[selected_cluster_index].aligned_bounding_box.angle; + float angle = feats[selected_cluster_index].oriented_bounding_box.angle; //std::cout << "Selected cluster angle (rad, deg): " << angle << " " << angle*180.0/3.14159 << std::endl; //std::cout << "Selected cluster hue: " << feats[selected_cluster_index].hue << std::endl; - hlpr_feature_extraction::PcFeatureArray rosMsg; + hlpr_perception_msgs::ExtractedFeaturesArray rosMsg; rosMsg.header.stamp = ros::Time::now(); for(int i = 0; i < feats.size(); i++) { - hlpr_feature_extraction::PcFeatures ft; - fillRosMessage(ft, feats[i]); + hlpr_perception_msgs::ObjectFeatures ft; + fillRosMessageForObjects(ft, feats[i]); rosMsg.objects.push_back(ft); rosMsg.transforms.push_back(ft.transform); } diff --git a/hlpr_feature_extraction/src/utils_pcl_ros.cpp b/hlpr_feature_extraction/src/utils_pcl_ros.cpp index bf06e6c..f36d3ea 100644 --- a/hlpr_feature_extraction/src/utils_pcl_ros.cpp +++ b/hlpr_feature_extraction/src/utils_pcl_ros.cpp @@ -45,18 +45,113 @@ Cube_2_Arrows(pcl::ModelCoefficients &cube, boost::shared_ptr Date: Tue, 1 Aug 2017 10:25:34 -0400 Subject: [PATCH 02/28] updated object labeling and knowledge retrieval for new perception messages --- hlpr_knowledge_retrieval/CMakeLists.txt | 4 ++-- hlpr_knowledge_retrieval/package.xml | 2 +- hlpr_object_labeling/CMakeLists.txt | 4 ++-- hlpr_object_labeling/msg/LabeledObjects.msg | 2 +- hlpr_object_labeling/package.xml | 2 +- hlpr_segmentation/msg/ClusterIndex.msg | 1 - hlpr_segmentation/msg/SegmentedClusters.msg | 5 ----- 7 files changed, 7 insertions(+), 13 deletions(-) delete mode 100644 hlpr_segmentation/msg/ClusterIndex.msg delete mode 100644 hlpr_segmentation/msg/SegmentedClusters.msg diff --git a/hlpr_knowledge_retrieval/CMakeLists.txt b/hlpr_knowledge_retrieval/CMakeLists.txt index b78f223..eb75ba0 100644 --- a/hlpr_knowledge_retrieval/CMakeLists.txt +++ b/hlpr_knowledge_retrieval/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(hlpr_knowledge_retrieval) -find_package(catkin REQUIRED COMPONENTS message_generation rospy std_msgs geometry_msgs sensor_msgs hlpr_feature_extraction) +find_package(catkin REQUIRED COMPONENTS message_generation rospy std_msgs geometry_msgs sensor_msgs hlpr_perception_msgs) add_message_files( FILES # e.g. Floats.msg HeaderString.msg @@ -19,7 +19,7 @@ generate_messages( ) catkin_package( - CATKIN_DEPENDS message_runtime std_msgs geometry_msgs sensor_msgs hlpr_feature_extraction + CATKIN_DEPENDS message_runtime std_msgs geometry_msgs sensor_msgs hlpr_perception_msgs ) diff --git a/hlpr_knowledge_retrieval/package.xml b/hlpr_knowledge_retrieval/package.xml index b4b858d..0863294 100644 --- a/hlpr_knowledge_retrieval/package.xml +++ b/hlpr_knowledge_retrieval/package.xml @@ -14,6 +14,6 @@ std_msgs geometry_msgs sensor_msgs - hlpr_feature_extraction + hlpr_perception_msgs diff --git a/hlpr_object_labeling/CMakeLists.txt b/hlpr_object_labeling/CMakeLists.txt index b7c7b2a..5ed3591 100644 --- a/hlpr_object_labeling/CMakeLists.txt +++ b/hlpr_object_labeling/CMakeLists.txt @@ -16,11 +16,11 @@ generate_messages( std_msgs geometry_msgs sensor_msgs - hlpr_feature_extraction + hlpr_perception_msgs ) catkin_package( - CATKIN_DEPENDS message_runtime std_msgs geometry_msgs sensor_msgs hlpr_feature_extraction + CATKIN_DEPENDS message_runtime std_msgs geometry_msgs sensor_msgs hlpr_perception_msgs ) diff --git a/hlpr_object_labeling/msg/LabeledObjects.msg b/hlpr_object_labeling/msg/LabeledObjects.msg index 14e6ab5..73e4c4d 100644 --- a/hlpr_object_labeling/msg/LabeledObjects.msg +++ b/hlpr_object_labeling/msg/LabeledObjects.msg @@ -1,3 +1,3 @@ Header header -hlpr_feature_extraction/PcFeatures[] objects +hlpr_perception_msgs/ObjectFeatures[] objects std_msgs/String[] labels diff --git a/hlpr_object_labeling/package.xml b/hlpr_object_labeling/package.xml index 7bae93a..30f56bc 100644 --- a/hlpr_object_labeling/package.xml +++ b/hlpr_object_labeling/package.xml @@ -13,6 +13,6 @@ std_msgs geometry_msgs sensor_msgs - hlpr_feature_extraction + hlpr_perception_msgs diff --git a/hlpr_segmentation/msg/ClusterIndex.msg b/hlpr_segmentation/msg/ClusterIndex.msg deleted file mode 100644 index c07888d..0000000 --- a/hlpr_segmentation/msg/ClusterIndex.msg +++ /dev/null @@ -1 +0,0 @@ -std_msgs/Int32[] indices diff --git a/hlpr_segmentation/msg/SegmentedClusters.msg b/hlpr_segmentation/msg/SegmentedClusters.msg deleted file mode 100644 index c91e5f9..0000000 --- a/hlpr_segmentation/msg/SegmentedClusters.msg +++ /dev/null @@ -1,5 +0,0 @@ -Header header -sensor_msgs/PointCloud2[] clusters -sensor_msgs/PointCloud2[] normals -std_msgs/Float32MultiArray plane -ClusterIndex[] cluster_ids From 9384652d135cb17fca9b7325c3d31849bb96824d Mon Sep 17 00:00:00 2001 From: TescaF Date: Wed, 2 Aug 2017 18:34:25 -0400 Subject: [PATCH 03/28] updated object labeling for new perception messages --- .../include/utils_pcl_ros.hpp | 3 +-- hlpr_object_labeling/src/object_recording.py | 21 +++++++++++-------- hlpr_segmentation/src/segmentation.cpp | 4 ++-- 3 files changed, 15 insertions(+), 13 deletions(-) diff --git a/hlpr_feature_extraction/include/utils_pcl_ros.hpp b/hlpr_feature_extraction/include/utils_pcl_ros.hpp index 531aab4..100cccb 100644 --- a/hlpr_feature_extraction/include/utils_pcl_ros.hpp +++ b/hlpr_feature_extraction/include/utils_pcl_ros.hpp @@ -17,7 +17,6 @@ #include #include #include -#include #include #include #include @@ -73,7 +72,7 @@ void objectPoseTF(geometry_msgs::Transform geom_transform); void -getObjectMarker(visualization_msgs::Marker &marker, hlpr_feature_extraction::PcFeatures &feats); +getObjectMarker(visualization_msgs::Marker &marker, hlpr_perception_msgs::ObjectFeatures &feats); template diff --git a/hlpr_object_labeling/src/object_recording.py b/hlpr_object_labeling/src/object_recording.py index c0a9242..2dbb704 100755 --- a/hlpr_object_labeling/src/object_recording.py +++ b/hlpr_object_labeling/src/object_recording.py @@ -9,7 +9,7 @@ import rospy import pdb from Tkinter import * -from hlpr_feature_extraction.msg import PcFeatureArray +from hlpr_perception_msgs.msg import ExtractedFeaturesArray pf = None display = None @@ -27,7 +27,7 @@ def get_param(name, value=None): class filter: def __init__(self): rospy.init_node('tracker', anonymous=True) - self.subscriber = rospy.Subscriber("/beliefs/features", PcFeatureArray, self.cbClusters, queue_size = 1) + self.subscriber = rospy.Subscriber("/beliefs/features", ExtractedFeaturesArray, self.cbClusters, queue_size = 1) self.initialized = False self.labeled = False self.labeledIdx = 0 @@ -44,11 +44,11 @@ def cbClusters(self, ros_data): return if self.initialized is False: self.initX = [] - for c in clusters: - size = c.bb_dims.x * c.bb_dims.y + for cluster in clusters: + size = cluster.obb.bb_dims.x * cluster.obb.bb_dims.y print 'Object size: ' + str(size) if size > self.minSize: - self.initX.append(c) + self.initX.append(cluster) if len(self.initX) is 0: return print str(len(self.initX)) + ' objects detected' @@ -56,13 +56,15 @@ def cbClusters(self, ros_data): elif self.labeled is False: var = raw_input("Enter label for object " + str(self.labeledIdx) + ": ") print "You entered: ", var - c = self.initX[self.labeledIdx] + cluster = self.initX[self.labeledIdx] + c = cluster.basicInfo + bb = cluster.obb r = c.rgba_color.r g = c.rgba_color.g b = c.rgba_color.b hsv = cv2.cvtColor(np.array([[(r,g,b)]],dtype='float32'), cv2.COLOR_RGB2HSV) - size = c.bb_dims.x * c.bb_dims.y + size = bb.bb_dims.x * bb.bb_dims.y self.outf.write(var + "," + str(hsv[0][0][0]) + "," + str(hsv[0][0][1]) + "," + str(hsv[0][0][2]) + "," + str(size) + "\n") self.labeledIdx = self.labeledIdx + 1 self.labeled = self.labeledIdx == len(self.initX) @@ -90,9 +92,10 @@ def drawClusters(self,clusters): return self.canvas.delete("all") for idx in range(0,len(clusters)): - c = clusters[idx] - if c is None: + cluster = clusters[idx] + if cluster is None: continue + c = cluster.basicInfo pts = [(c.points_min.x,c.points_min.y),(c.points_min.x,c.points_max.y),(c.points_max.x,c.points_max.y),(c.points_max.x,c.points_min.y)] offset = complex(c.points_centroid.x,c.points_centroid.y) cangle = 0 # cmath.exp(c.angle*1j) diff --git a/hlpr_segmentation/src/segmentation.cpp b/hlpr_segmentation/src/segmentation.cpp index b48e4c5..86795f0 100644 --- a/hlpr_segmentation/src/segmentation.cpp +++ b/hlpr_segmentation/src/segmentation.cpp @@ -170,8 +170,8 @@ main (int argc, char **argv) multi_plane_app.setViewer(viewer); } - // float workSpace[] = {-0.3,0.4,-0.25,0.35,0.3,2.0}; - float workSpace[] = {-0.55,0.5,-0.2,0.45,0.3,2.0}; + //float workSpace[] = {-0.55,0.5,-0.2,0.45,0.3,2.0}; + float workSpace[] = {-0.55,0.5,0.0,0.35,0.3,2.0}; multi_plane_app.setWorkingVolumeThresholds(workSpace); //if(pA.output_type == comType::cROS) From 4da28fd7eaee401fae3595d1357a0cfb9efa0c19 Mon Sep 17 00:00:00 2001 From: TescaF Date: Thu, 3 Aug 2017 11:40:28 -0400 Subject: [PATCH 04/28] updated object labeling and knowledge retrieval for new message types. added labeling and knowledge messages to hlpr_perception_msgs --- .../include/utils_pcl_ros.hpp | 3 +-- hlpr_knowledge_retrieval/CMakeLists.txt | 15 ------------- hlpr_object_labeling/CMakeLists.txt | 14 ------------- hlpr_object_labeling/src/object_labeling.py | 18 ++++++++-------- hlpr_object_labeling/src/object_recording.py | 21 +++++++++++-------- hlpr_perception_msgs/CMakeLists.txt | 4 ++++ .../msg/LabeledObjects.msg | 0 .../msg/ObjectKnowledge.msg | 0 .../msg/ObjectKnowledgeArray.msg | 0 9 files changed, 26 insertions(+), 49 deletions(-) rename {hlpr_object_labeling => hlpr_perception_msgs}/msg/LabeledObjects.msg (100%) rename {hlpr_knowledge_retrieval => hlpr_perception_msgs}/msg/ObjectKnowledge.msg (100%) rename {hlpr_knowledge_retrieval => hlpr_perception_msgs}/msg/ObjectKnowledgeArray.msg (100%) diff --git a/hlpr_feature_extraction/include/utils_pcl_ros.hpp b/hlpr_feature_extraction/include/utils_pcl_ros.hpp index 531aab4..100cccb 100644 --- a/hlpr_feature_extraction/include/utils_pcl_ros.hpp +++ b/hlpr_feature_extraction/include/utils_pcl_ros.hpp @@ -17,7 +17,6 @@ #include #include #include -#include #include #include #include @@ -73,7 +72,7 @@ void objectPoseTF(geometry_msgs::Transform geom_transform); void -getObjectMarker(visualization_msgs::Marker &marker, hlpr_feature_extraction::PcFeatures &feats); +getObjectMarker(visualization_msgs::Marker &marker, hlpr_perception_msgs::ObjectFeatures &feats); template diff --git a/hlpr_knowledge_retrieval/CMakeLists.txt b/hlpr_knowledge_retrieval/CMakeLists.txt index eb75ba0..27342f5 100644 --- a/hlpr_knowledge_retrieval/CMakeLists.txt +++ b/hlpr_knowledge_retrieval/CMakeLists.txt @@ -3,21 +3,6 @@ project(hlpr_knowledge_retrieval) find_package(catkin REQUIRED COMPONENTS message_generation rospy std_msgs geometry_msgs sensor_msgs hlpr_perception_msgs) -add_message_files( - FILES # e.g. Floats.msg HeaderString.msg - #SegmentedClusters.msg - ObjectKnowledge.msg - ObjectKnowledgeArray.msg -) - -## Generate added messages and services with any dependencies -generate_messages( - DEPENDENCIES - std_msgs - geometry_msgs - sensor_msgs -) - catkin_package( CATKIN_DEPENDS message_runtime std_msgs geometry_msgs sensor_msgs hlpr_perception_msgs ) diff --git a/hlpr_object_labeling/CMakeLists.txt b/hlpr_object_labeling/CMakeLists.txt index 5ed3591..e8253f2 100644 --- a/hlpr_object_labeling/CMakeLists.txt +++ b/hlpr_object_labeling/CMakeLists.txt @@ -5,20 +5,6 @@ find_package(catkin REQUIRED COMPONENTS message_generation rospy std_msgs geomet #catkin_python_setup() -add_message_files( - FILES - LabeledObjects.msg -) - -## Generate added messages and services with any dependencies -generate_messages( - DEPENDENCIES - std_msgs - geometry_msgs - sensor_msgs - hlpr_perception_msgs -) - catkin_package( CATKIN_DEPENDS message_runtime std_msgs geometry_msgs sensor_msgs hlpr_perception_msgs ) diff --git a/hlpr_object_labeling/src/object_labeling.py b/hlpr_object_labeling/src/object_labeling.py index abe7781..b819978 100755 --- a/hlpr_object_labeling/src/object_labeling.py +++ b/hlpr_object_labeling/src/object_labeling.py @@ -11,8 +11,7 @@ import tf import itertools from Tkinter import * -from hlpr_feature_extraction.msg import PcFeatureArray -from hlpr_object_labeling.msg import LabeledObjects +from hlpr_perception_msgs.msg import ObjectFeatures, ExtractedFeaturesArray, LabeledObjects from std_msgs.msg import String pf = None @@ -52,7 +51,7 @@ def __init__(self): if topicref is not None: self.rostopic = os.path.expanduser(topicref) self.fileSub = rospy.Subscriber(self.rostopic, String, self.cbFile, queue_size = 1) - self.subscriber = rospy.Subscriber("/beliefs/features", PcFeatureArray, self.cbClusters, queue_size = 1) + self.subscriber = rospy.Subscriber("/beliefs/features", ExtractedFeaturesArray, self.cbClusters, queue_size = 1) self.pauseSub = rospy.Subscriber("/pause_labeling", String, self.cbPause, queue_size = 1) self.orderPub = rospy.Publisher("/beliefs/labels", LabeledObjects, queue_size = 1) @@ -136,9 +135,9 @@ def loadObjects(self): def hsvDiff(self, c1,c2): hsv1 = c1[1:4] - r2 = c2.rgba_color.r - g2 = c2.rgba_color.g - b2 = c2.rgba_color.b + r2 = c2.basicInfo.rgba_color.r + g2 = c2.basicInfo.rgba_color.g + b2 = c2.basicInfo.rgba_color.b hsv2 = cv2.cvtColor(np.array([[(r2,g2,b2)]],dtype='float32'), cv2.COLOR_RGB2HSV) h1 = hsv2[0][0][0] h2 = float(hsv1[0]) @@ -147,7 +146,7 @@ def hsvDiff(self, c1,c2): return abs(huediff), abs(hsv2[0][0][1]-float(hsv1[1])), abs(hsv2[0][0][2]-float(hsv1[2])) def sizeDiff(self, c1,c2): - size = c2.bb_dims.x * c2.bb_dims.y + size = c2.obb.bb_dims.x * c2.obb.bb_dims.y return abs(size - float(c1[4])) def calculateError(self, init, cluster): @@ -223,9 +222,10 @@ def drawClusters(self,clusters,ids): return self.canvas.delete("all") for idx in range(0,len(clusters)): - c = clusters[idx] - if c is None: + cluster = clusters[idx] + if cluster is None: continue + c = cluster.basicInfo pts = [(c.points_min.x,c.points_min.y),(c.points_min.x,c.points_max.y),(c.points_max.x,c.points_max.y),(c.points_max.x,c.points_min.y)] offset = complex(c.points_centroid.x,c.points_centroid.y) cangle = 0 # cmath.exp(c.angle*1j) diff --git a/hlpr_object_labeling/src/object_recording.py b/hlpr_object_labeling/src/object_recording.py index c0a9242..2dbb704 100755 --- a/hlpr_object_labeling/src/object_recording.py +++ b/hlpr_object_labeling/src/object_recording.py @@ -9,7 +9,7 @@ import rospy import pdb from Tkinter import * -from hlpr_feature_extraction.msg import PcFeatureArray +from hlpr_perception_msgs.msg import ExtractedFeaturesArray pf = None display = None @@ -27,7 +27,7 @@ def get_param(name, value=None): class filter: def __init__(self): rospy.init_node('tracker', anonymous=True) - self.subscriber = rospy.Subscriber("/beliefs/features", PcFeatureArray, self.cbClusters, queue_size = 1) + self.subscriber = rospy.Subscriber("/beliefs/features", ExtractedFeaturesArray, self.cbClusters, queue_size = 1) self.initialized = False self.labeled = False self.labeledIdx = 0 @@ -44,11 +44,11 @@ def cbClusters(self, ros_data): return if self.initialized is False: self.initX = [] - for c in clusters: - size = c.bb_dims.x * c.bb_dims.y + for cluster in clusters: + size = cluster.obb.bb_dims.x * cluster.obb.bb_dims.y print 'Object size: ' + str(size) if size > self.minSize: - self.initX.append(c) + self.initX.append(cluster) if len(self.initX) is 0: return print str(len(self.initX)) + ' objects detected' @@ -56,13 +56,15 @@ def cbClusters(self, ros_data): elif self.labeled is False: var = raw_input("Enter label for object " + str(self.labeledIdx) + ": ") print "You entered: ", var - c = self.initX[self.labeledIdx] + cluster = self.initX[self.labeledIdx] + c = cluster.basicInfo + bb = cluster.obb r = c.rgba_color.r g = c.rgba_color.g b = c.rgba_color.b hsv = cv2.cvtColor(np.array([[(r,g,b)]],dtype='float32'), cv2.COLOR_RGB2HSV) - size = c.bb_dims.x * c.bb_dims.y + size = bb.bb_dims.x * bb.bb_dims.y self.outf.write(var + "," + str(hsv[0][0][0]) + "," + str(hsv[0][0][1]) + "," + str(hsv[0][0][2]) + "," + str(size) + "\n") self.labeledIdx = self.labeledIdx + 1 self.labeled = self.labeledIdx == len(self.initX) @@ -90,9 +92,10 @@ def drawClusters(self,clusters): return self.canvas.delete("all") for idx in range(0,len(clusters)): - c = clusters[idx] - if c is None: + cluster = clusters[idx] + if cluster is None: continue + c = cluster.basicInfo pts = [(c.points_min.x,c.points_min.y),(c.points_min.x,c.points_max.y),(c.points_max.x,c.points_max.y),(c.points_max.x,c.points_min.y)] offset = complex(c.points_centroid.x,c.points_centroid.y) cangle = 0 # cmath.exp(c.angle*1j) diff --git a/hlpr_perception_msgs/CMakeLists.txt b/hlpr_perception_msgs/CMakeLists.txt index c571951..97028d4 100644 --- a/hlpr_perception_msgs/CMakeLists.txt +++ b/hlpr_perception_msgs/CMakeLists.txt @@ -27,6 +27,10 @@ add_message_files( ObjectFeatures.msg PlaneFeatures.msg ExtractedFeaturesArray.msg + # Knowledge feature msgs + LabeledObjects.msg + ObjectKnowledge.msg + ObjectKnowledgeArray.msg ) generate_messages( diff --git a/hlpr_object_labeling/msg/LabeledObjects.msg b/hlpr_perception_msgs/msg/LabeledObjects.msg similarity index 100% rename from hlpr_object_labeling/msg/LabeledObjects.msg rename to hlpr_perception_msgs/msg/LabeledObjects.msg diff --git a/hlpr_knowledge_retrieval/msg/ObjectKnowledge.msg b/hlpr_perception_msgs/msg/ObjectKnowledge.msg similarity index 100% rename from hlpr_knowledge_retrieval/msg/ObjectKnowledge.msg rename to hlpr_perception_msgs/msg/ObjectKnowledge.msg diff --git a/hlpr_knowledge_retrieval/msg/ObjectKnowledgeArray.msg b/hlpr_perception_msgs/msg/ObjectKnowledgeArray.msg similarity index 100% rename from hlpr_knowledge_retrieval/msg/ObjectKnowledgeArray.msg rename to hlpr_perception_msgs/msg/ObjectKnowledgeArray.msg From f30e7cfbf475a0cc7e1abc056d2dcdf31888cf42 Mon Sep 17 00:00:00 2001 From: TescaF Date: Thu, 3 Aug 2017 11:41:35 -0400 Subject: [PATCH 05/28] adjusted workspace size --- hlpr_segmentation/src/segmentation.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/hlpr_segmentation/src/segmentation.cpp b/hlpr_segmentation/src/segmentation.cpp index b48e4c5..86795f0 100644 --- a/hlpr_segmentation/src/segmentation.cpp +++ b/hlpr_segmentation/src/segmentation.cpp @@ -170,8 +170,8 @@ main (int argc, char **argv) multi_plane_app.setViewer(viewer); } - // float workSpace[] = {-0.3,0.4,-0.25,0.35,0.3,2.0}; - float workSpace[] = {-0.55,0.5,-0.2,0.45,0.3,2.0}; + //float workSpace[] = {-0.55,0.5,-0.2,0.45,0.3,2.0}; + float workSpace[] = {-0.55,0.5,0.0,0.35,0.3,2.0}; multi_plane_app.setWorkingVolumeThresholds(workSpace); //if(pA.output_type == comType::cROS) From 04a0c8f25cbdc48eff128111533b1920df0080f2 Mon Sep 17 00:00:00 2001 From: TescaF Date: Thu, 3 Aug 2017 12:35:02 -0400 Subject: [PATCH 06/28] removed unnecessary package --- hlpr_object_labeling/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hlpr_object_labeling/CMakeLists.txt b/hlpr_object_labeling/CMakeLists.txt index e8253f2..aa62217 100644 --- a/hlpr_object_labeling/CMakeLists.txt +++ b/hlpr_object_labeling/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(hlpr_object_labeling) -find_package(catkin REQUIRED COMPONENTS message_generation rospy std_msgs geometry_msgs sensor_msgs hlpr_feature_extraction) +find_package(catkin REQUIRED COMPONENTS message_generation rospy std_msgs geometry_msgs sensor_msgs hlpr_perception_msgs) #catkin_python_setup() From c0c65c9bdcfb030d11a349e46a6e82a8d248ad42 Mon Sep 17 00:00:00 2001 From: TescaF Date: Thu, 3 Aug 2017 17:12:37 -0400 Subject: [PATCH 07/28] updated object label viewer and file referencing --- .../src/object_knowledge_retrieval.py | 16 +++++++--------- hlpr_object_labeling/src/object_labeling.py | 14 +++++++------- 2 files changed, 14 insertions(+), 16 deletions(-) diff --git a/hlpr_knowledge_retrieval/src/object_knowledge_retrieval.py b/hlpr_knowledge_retrieval/src/object_knowledge_retrieval.py index 8a7a45c..0fda976 100755 --- a/hlpr_knowledge_retrieval/src/object_knowledge_retrieval.py +++ b/hlpr_knowledge_retrieval/src/object_knowledge_retrieval.py @@ -7,8 +7,7 @@ import roslib import rospy -from hlpr_object_labeling.msg import LabeledObjects -from hlpr_knowledge_retrieval.msg import ObjectKnowledge, ObjectKnowledgeArray +from hlpr_perception_msgs.msg import LabeledObjects, ObjectKnowledge, ObjectKnowledgeArray def get_param(name, value=None): private = "~%s" % name @@ -24,17 +23,16 @@ def __init__(self): self.subscriber = rospy.Subscriber("/beliefs/labels", LabeledObjects, self.cbLabels, queue_size = 1) self.knowPub = rospy.Publisher("/beliefs/knowledge", ObjectKnowledge) - fileref = get_param("data_file_location") - if fileref is not None: - self.filename = os.path.expanduser(fileref) - self.readObjectKnowledge(self.filename) - print "Reading knowledge data from " + self.filename - else: - self.filename = None topicref = get_param("data_file_rostopic") + fileref = get_param("data_file_location") + self.filename = None if topicref is not None: self.rostopic = os.path.expanduser(topicref) self.fileSub = rospy.Subscriber(self.rostopic, String, self.cbFile, queue_size = 1) + elif fileref is not None: + self.filename = os.path.expanduser(fileref) + self.readObjectKnowledge(self.filename) + print "Reading knowledge data from " + self.filename def cbFile(self, ros_data): if self.filename is not ros_data.data: diff --git a/hlpr_object_labeling/src/object_labeling.py b/hlpr_object_labeling/src/object_labeling.py index b819978..5add1b7 100755 --- a/hlpr_object_labeling/src/object_labeling.py +++ b/hlpr_object_labeling/src/object_labeling.py @@ -43,14 +43,13 @@ def __init__(self): self.valW = get_param("hsv_val_weight",1) self.sizeW = get_param("size_weight",50000) fileref = get_param("feature_file_location") - if fileref is not None: - self.filename = os.path.expanduser(fileref) - else: - self.filename = None topicref = get_param("feature_file_rostopic") + self.filename = None if topicref is not None: self.rostopic = os.path.expanduser(topicref) self.fileSub = rospy.Subscriber(self.rostopic, String, self.cbFile, queue_size = 1) + elif fileref is not None: + self.filename = os.path.expanduser(fileref) self.subscriber = rospy.Subscriber("/beliefs/features", ExtractedFeaturesArray, self.cbClusters, queue_size = 1) self.pauseSub = rospy.Subscriber("/pause_labeling", String, self.cbPause, queue_size = 1) self.orderPub = rospy.Publisher("/beliefs/labels", LabeledObjects, queue_size = 1) @@ -232,11 +231,12 @@ def drawClusters(self,clusters,ids): rot = [] for x,y in pts: r = cangle * (complex(x,y)-offset) + offset - rot.append((-r.real + 0.5) * 500) - rot.append((-r.imag + 0.5) * 500) + rot.append((r.real + 0.5) * 500) + rot.append((r.imag + 0.5) * 500) rgb = '#%02x%02x%02x' % (c.rgba_color.r,c.rgba_color.g,c.rgba_color.b) poly = self.canvas.create_polygon(rot,outline=rgb,fill='white',width=5) - label = self.canvas.create_text((-c.points_centroid.x+0.5)*500, (-c.points_centroid.y + 0.5)*500,text=str(ids[idx].data),font="Verdana 10 bold") + label = self.canvas.create_text((c.points_centroid.x+0.5)*500, (c.points_centroid.y + 0.5)*500,text=str(ids[idx].data),font="Verdana 10 bold") + #label = self.canvas.create_text((-c.points_centroid.x+0.5)*500, (-c.points_centroid.y + 0.5)*500,text=str(ids[idx].data),font="Verdana 10 bold") self.canvas.pack() def main(args): From f62a9449cb7a4ff81f31d8b7900fc4df54f6edad Mon Sep 17 00:00:00 2001 From: TescaF Date: Tue, 8 Aug 2017 18:54:06 -0400 Subject: [PATCH 08/28] updated direction of object recording display --- hlpr_object_labeling/src/object_recording.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/hlpr_object_labeling/src/object_recording.py b/hlpr_object_labeling/src/object_recording.py index 2dbb704..880e9c4 100755 --- a/hlpr_object_labeling/src/object_recording.py +++ b/hlpr_object_labeling/src/object_recording.py @@ -102,11 +102,11 @@ def drawClusters(self,clusters): rot = [] for x,y in pts: r = cangle * (complex(x,y)-offset) + offset - rot.append((-r.real + 0.5) * 500) - rot.append((-r.imag + 0.5) * 500) + rot.append((r.real + 0.5) * 500) + rot.append((r.imag + 0.5) * 500) rgb = '#%02x%02x%02x' % (c.rgba_color.r,c.rgba_color.g,c.rgba_color.b) poly = self.canvas.create_polygon(rot,outline=rgb,fill='white',width=5) - label = self.canvas.create_text((-c.points_centroid.x+0.5)*500, (-c.points_centroid.y + 0.5)*500,text=str(idx),font="Verdana 10 bold") + label = self.canvas.create_text((c.points_centroid.x+0.5)*500, (c.points_centroid.y + 0.5)*500,text=str(idx),font="Verdana 10 bold") self.canvas.pack() def main(args): From 821c1740db0477191943efe97a180996e783b275 Mon Sep 17 00:00:00 2001 From: TescaF Date: Wed, 9 Aug 2017 11:57:01 -0400 Subject: [PATCH 09/28] disabled excess print messages after labeling --- hlpr_object_labeling/src/object_recording.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/hlpr_object_labeling/src/object_recording.py b/hlpr_object_labeling/src/object_recording.py index 880e9c4..47cc5d0 100755 --- a/hlpr_object_labeling/src/object_recording.py +++ b/hlpr_object_labeling/src/object_recording.py @@ -33,6 +33,7 @@ def __init__(self): self.labeledIdx = 0 self.initX = [] self.finished = False + self.exit = False self.filename = os.path.expanduser(get_param("feature_file_location", "tracked_object_data.txt")) self.minSize = get_param("min_object_size", 0.001) self.outf = open(self.filename, "w") @@ -71,8 +72,9 @@ def cbClusters(self, ros_data): elif self.finished is False: self.outf.close() self.finished = True - elif self.finished is True: + elif self.finished is True and self.exit is False: print "Objects written to " + str(self.filename) + self.exit = True sys.exit() class ui: From f2f9b64cfa035bd07631339840b5ceccad350666 Mon Sep 17 00:00:00 2001 From: RAIL Date: Tue, 10 Oct 2017 16:51:33 -0400 Subject: [PATCH 10/28] add in vfh features in this branch --- hlpr_feature_extraction/src/utils_pcl_ros.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/hlpr_feature_extraction/src/utils_pcl_ros.cpp b/hlpr_feature_extraction/src/utils_pcl_ros.cpp index f36d3ea..f23b369 100644 --- a/hlpr_feature_extraction/src/utils_pcl_ros.cpp +++ b/hlpr_feature_extraction/src/utils_pcl_ros.cpp @@ -117,6 +117,10 @@ fillObjectFeaturesMsg (hlpr_perception_msgs::ObjectFeatures &objRosMsg, objRosMsg.setOtherFeatures = inObjFeatures.setOtherFeatures; objRosMsg.other = other; + + objRosMsg.other.other_features_size = 308; + for(int i = 0;i Date: Thu, 14 Dec 2017 15:17:09 -0600 Subject: [PATCH 11/28] Deleted temp file --- hlpr_segmentation/src/segmentation.cpp~ | 330 ------------------------ 1 file changed, 330 deletions(-) delete mode 100644 hlpr_segmentation/src/segmentation.cpp~ diff --git a/hlpr_segmentation/src/segmentation.cpp~ b/hlpr_segmentation/src/segmentation.cpp~ deleted file mode 100644 index a5bf711..0000000 --- a/hlpr_segmentation/src/segmentation.cpp~ +++ /dev/null @@ -1,330 +0,0 @@ -/* - * ircp_loop.cpp - * - * Created on: Sep 4, 2014 - * Author: baris - */ - -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -//#include - -#include - -#include - -pcl::PointCloud::ConstPtr prev_cloud; -pcl::PointCloud cloud_store; -boost::mutex cloud_mutex; -boost::mutex imageName_mutex; -bool writePCD2File = false; -char imageName[100]; -bool gotFirst = false; -bool interrupt = false; -const char *clusterOutRostopic = "/beliefs/clusters"; -const char *normalOutRostopic = "/beliefs/normals"; -const char *planeOutRostopic = "/beliefs/plane"; -const char *segOutRostopic = "/beliefs/clusters"; -bool viz_ = false; - -void -cloud_cb_ros_ (const sensor_msgs::PointCloud2ConstPtr& msg) -{ - pcl::PCLPointCloud2 pcl_pc; - - if (msg->height == 1){ - sensor_msgs::PointCloud2 new_cloud_msg; - new_cloud_msg.header = msg->header; - new_cloud_msg.height = 480; - new_cloud_msg.width = 640; - new_cloud_msg.fields = msg->fields; - new_cloud_msg.is_bigendian = msg->is_bigendian; - new_cloud_msg.point_step = msg->point_step; - new_cloud_msg.row_step = 20480; - new_cloud_msg.data = msg->data; - new_cloud_msg.is_dense = msg->is_dense; - - pcl_conversions::toPCL(new_cloud_msg, pcl_pc); - } - else - pcl_conversions::toPCL(*msg, pcl_pc); - - pcl::PointCloud cloud; - pcl::fromPCLPointCloud2(pcl_pc, cloud); - pcl::copyPointCloud(cloud, cloud_store); - - cloud_mutex.lock (); - prev_cloud = cloud.makeShared(); - if(writePCD2File) - { - pcl::PointCloud::Ptr saved_cloud(new pcl::PointCloud(*prev_cloud)); - std::cout << imageName << std::endl; - cloud_mutex.unlock (); - imageName_mutex.lock(); - pcl::io::savePCDFile(imageName, *saved_cloud); - imageName_mutex.unlock(); - writePCD2File = false; - } - else - cloud_mutex.unlock (); - - gotFirst = true; -} - -void interruptFn(int sig) { - interrupt = true; -} - -void -cloud_cb_direct_ (const pcl::PointCloud::ConstPtr& cloud) -{ - cloud_mutex.lock (); - prev_cloud = cloud; - if(writePCD2File) - { - pcl::PointCloud::Ptr saved_cloud(new pcl::PointCloud(*prev_cloud)); - std::cout << imageName << std::endl; - cloud_mutex.unlock (); - imageName_mutex.lock(); - pcl::io::savePCDFile(imageName, *saved_cloud); - imageName_mutex.unlock(); - writePCD2File = false; - } - else - cloud_mutex.unlock (); - - gotFirst = true; -} - -inline void -fake_cloud_cb_kinectv2_ (const pcl::PointCloud::ConstPtr& cloud) -{ - prev_cloud = cloud; - if(writePCD2File) - { - pcl::PointCloud::Ptr saved_cloud(new pcl::PointCloud(*prev_cloud)); - std::cout << imageName << std::endl; - cloud_mutex.unlock (); - imageName_mutex.lock(); - pcl::io::savePCDFile(imageName, *saved_cloud); - imageName_mutex.unlock(); - writePCD2File = false; - } -} - -int -main (int argc, char **argv) -{ - std::cout << "main function" << std::endl; - pcl::Grabber* interface; - ros::NodeHandle *nh; - ros::Subscriber sub; - ros::Publisher clusterPub, normalPub, planePub,msgPub; - - bool spawnObject = true; - - K2G *k2g; - processor freenectprocessor = OPENGL; - - boost::shared_ptr> cloud; - - std::cout << "ros node initialized" << std::endl; - ros::init(argc, argv, "hlpr_segmentation",ros::init_options::NoSigintHandler); - nh = new ros::NodeHandle("~"); - // nh->getParam("segmentation/viz", viz_); - // std::cout<<"viz is set to " << viz_ << endl; - - parsedArguments pA; - if(parseArguments(argc, argv, pA, *nh) < 0) - return 0; - - viz_ = pA.viz; - ridiculous_global_variables::ignore_low_sat = pA.saturation_hack; - ridiculous_global_variables::saturation_threshold = pA.saturation_hack_value; - ridiculous_global_variables::saturation_mapped_value = pA.saturation_mapped_value; - - OpenNIOrganizedMultiPlaneSegmentation multi_plane_app; - - pcl::PointCloud::Ptr cloud_ptr (new pcl::PointCloud); - pcl::PointCloud::Ptr ncloud_ptr (new pcl::PointCloud); - pcl::PointCloud::Ptr label_ptr (new pcl::PointCloud); - - boost::shared_ptr viewer; - multi_plane_app.initSegmentation(pA.seg_color_ind, pA.ecc_dist_thresh, pA.ecc_color_thresh); - if(viz_) { - viewer = cloudViewer(cloud_ptr); - multi_plane_app.setViewer(viewer); - } - - // float workSpace[] = {-0.3,0.4,-0.25,0.35,0.3,2.0}; - float workSpace[] = {-0.55,0.5,-0.2,0.45,0.3,2.0}; - multi_plane_app.setWorkingVolumeThresholds(workSpace); - - //if(pA.output_type == comType::cROS) - //{ - //pub = nh->advertise(outRostopic,5); - // clusterPub = nh->advertise>(clusterOutRostopic,5); - // normalPub = nh->advertise>(normalOutRostopic,5); - // planePub = nh->advertise(planeOutRostopic,5); - msgPub = nh->advertise(segOutRostopic,5); - // } - - switch (pA.pc_source) - { - case 0: - { - std::cout << "Using ros topic as input" << std::endl; - sub = nh->subscribe(pA.ros_topic, 1, cloud_cb_ros_); - break; - } - case 1: - { - std::cout << "Using the openni device" << std::endl; - - interface = new pcl::OpenNIGrabber (); - - boost::function::ConstPtr&)> f = boost::bind (&cloud_cb_direct_,_1); - boost::signals2::connection c = interface->registerCallback (f); - - interface->start (); - break; - } - case 2: - default: - { - std::cout << "Using kinect v2" << std::endl; - - freenectprocessor = static_cast(pA.freenectProcessor); - - k2g = new K2G(freenectprocessor, true); - cloud = k2g->getCloud(); - prev_cloud = cloud; - gotFirst = true; - break; - } - } - - signal(SIGINT, interruptFn); - while (!interrupt && (!viz_ || !viewer->wasStopped ())) - { - if(pA.ros_node) - { - ros::spinOnce(); - } - if(viz_) - viewer->spinOnce(20); - if(!gotFirst) - continue; - int selected_cluster_index = -1; - if(cloud_mutex.try_lock ()) - { - if(pA.pc_source == 2) - { - cloud = k2g->getCloud(); - fake_cloud_cb_kinectv2_(cloud); - } - - if(viz_ && pA.justViewPointCloud) - { - pcl::PointCloud::Ptr filtered_prev_cloud(new pcl::PointCloud(*prev_cloud)); - multi_plane_app.preProcPointCloud(filtered_prev_cloud); - if (!viewer->updatePointCloud (filtered_prev_cloud, "cloud")) - { - viewer->addPointCloud (filtered_prev_cloud, "cloud"); - } - selected_cluster_index = -1; - } - else - { - pcl::PointCloud::CloudVectorType clusters; - std::vector > clusterNormals; - Eigen::Vector4f plane; - std::vector clusterIndices; - std::vector> clusterIndicesStore; - - selected_cluster_index = multi_plane_app.processOnce(prev_cloud,clusters,clusterNormals,plane,clusterIndicesStore, - pA.pre_proc, - pA.merge_clusters, viz_, pA.filterNoise); //true is for the viewer - hlpr_segmentation::SegmentedClusters msg; - msg.header.stamp = ros::Time::now(); - - // Pull out the cluster indices and put in msg - for (int ti = 0; ti < clusterIndicesStore.size(); ti++){ - hlpr_segmentation::ClusterIndex cluster_idx_msg; - for (int j = 0; j < clusterIndicesStore[ti].size(); j++){ - std_msgs::Int32 temp_msg; - temp_msg.data = clusterIndicesStore[ti][j]; - cluster_idx_msg.indices.push_back(temp_msg); - } - msg.cluster_ids.push_back(cluster_idx_msg); - //clusterIndices.insert(clusterIndices.end(),clusterIndicesStore[ti].begin(), clusterIndicesStore[ti].end()); // For removing ALL cluster points - } - - for(int i = 0; i < clusters.size(); i++) { - sensor_msgs::PointCloud2 out; - pcl::PCLPointCloud2 tmp; - pcl::toPCLPointCloud2(clusters[i], tmp); - pcl_conversions::fromPCL(tmp, out); - msg.clusters.push_back(out); - } - - for(int i = 0; i < clusterNormals.size(); i++) { - sensor_msgs::PointCloud2 out; - pcl::PCLPointCloud2 tmp; - pcl::toPCLPointCloud2(clusterNormals[i], tmp); - pcl_conversions::fromPCL(tmp, out); - msg.normals.push_back(out); - } - - std_msgs::Float32MultiArray planeMsg; - planeMsg.data.clear(); - for(int i = 0; i < 4; i++) - planeMsg.data.push_back(plane[i]); - //planePub.publish(planeMsg); - msg.plane = planeMsg; - - msgPub.publish(msg); - - /*clusterPub.publish(clusters[0]); - normalPub.publish(clusterNormals[0]); - std_msgs::Float32MultiArray planeMsg; - planeMsg.data.clear(); - for(int i = 0; i < 4; i++) - planeMsg.data.push_back(plane[i]); - planePub.publish(planeMsg);*/ - } - - cloud_mutex.unlock(); - - /*the z_thresh may result in wrong cluster to be selected. it might be a good idea to do - * some sort of mean shift tracking, or selecting the biggest amongst the candidates (vs choosing the most similar color) - * or sending all the plausible ones to c6 and decide there - */ - } - - if(selected_cluster_index < 0) - continue; - } - if(pA.pc_source == 1) - { - interface->stop (); - delete interface; - } - delete nh; - - ros::shutdown(); - return 0; -} From 08ac283d85844fd7a2d94828127399f061e8e0ce Mon Sep 17 00:00:00 2001 From: Adam Allevato Date: Thu, 14 Dec 2017 15:17:46 -0600 Subject: [PATCH 12/28] Fixed build errors found while trying to build in Kinetic. Three primary errors: 1. In all copies of utils.hpp, the definition of the method cannot have default arguments, because the defaults are already provided in the method declarations. This error may be cropping up because I'm using a newer compiler. 2. when listing link libraries, we were using hardcoded paths via the ROS_LIBRARIES CMake variable. This is provided automatically by the catkin_LIBRARIES variable, so I switched to using that instead in all the CMakeLists files. 3. hlpr_perception_msgs was not included as a dependency in all packages. --- hlpr_feature_extraction/CMakeLists.txt | 26 +++++++++---------- hlpr_feature_extraction/include/utils.hpp | 12 ++++----- .../CMakeLists.txt | 25 +++++++++--------- .../include/utils.hpp | 12 ++++----- hlpr_nonplanar_feature_extraction/package.xml | 1 + hlpr_nonplanar_segmentation/CMakeLists.txt | 23 ++++++++-------- hlpr_nonplanar_segmentation/include/utils.hpp | 12 ++++----- hlpr_nonplanar_segmentation/package.xml | 1 + hlpr_perception_msgs/CMakeLists.txt | 6 ++--- hlpr_segmentation/CMakeLists.txt | 20 +++++++------- hlpr_segmentation/include/utils.hpp | 12 ++++----- hlpr_single_plane_segmentation/CMakeLists.txt | 15 ++++++++--- .../include/utils.hpp | 12 ++++----- 13 files changed, 92 insertions(+), 85 deletions(-) diff --git a/hlpr_feature_extraction/CMakeLists.txt b/hlpr_feature_extraction/CMakeLists.txt index 687a556..19fa287 100644 --- a/hlpr_feature_extraction/CMakeLists.txt +++ b/hlpr_feature_extraction/CMakeLists.txt @@ -6,15 +6,6 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native") set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,--as-needed -pthread") set(OPENNI_INCLUDE_DIRS /usr/include/ni) -set(ROS_DISTRO indigo) -set(ROS_LIBRARIES /opt/ros/${ROS_DISTRO}/lib/libroscpp.so - /opt/ros/${ROS_DISTRO}/lib/libroscpp_serialization.so - /opt/ros/${ROS_DISTRO}/lib/librosconsole.so - /opt/ros/${ROS_DISTRO}/lib/libroslib.so - /opt/ros/${ROS_DISTRO}/lib/librostime.so - /opt/ros/${ROS_DISTRO}/lib/libtf2.so - /opt/ros/${ROS_DISTRO}/lib/libtf2_ros.so) - #find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure message_generation roscpp std_msgs geometry_msgs) find_package(OpenCL QUIET) @@ -24,8 +15,15 @@ IF(OpenCL_FOUND) ${LIBRARIES} ) ENDIF(OpenCL_FOUND) -find_package(catkin REQUIRED COMPONENTS message_generation roscpp std_msgs geometry_msgs tf2_ros hlpr_segmentation) -find_package(PCL 1.8 REQUIRED COMPONENTS) +find_package(catkin REQUIRED COMPONENTS + message_generation + roscpp + std_msgs + geometry_msgs + tf2_ros + hlpr_segmentation + pcl_ros +) find_package(freenect2 REQUIRED) find_package(OpenCV 2 REQUIRED) @@ -60,13 +58,13 @@ include_directories(include ${catkin_INCLUDE_DIRS}) include_directories(${PROJECT_SOURCE_DIR}/include) add_library(ftex SHARED src/cluster_processing.cpp src/rotcalipers.cpp src/utils.cpp src/utils_pcl_ros.cpp) -target_link_libraries(ftex ${PCL_LIBRARIES} ${ROS_LIBRARIES}) +target_link_libraries(ftex ${PCL_LIBRARIES} ${catkin_LIBRARIES}) add_dependencies(ftex hlpr_feature_extraction_generate_messages_cpp) add_executable(ft_ex src/feature_extraction.cpp) #add_executable(listener src/listener.cpp src/nodes/listener_node.cpp) -#target_link_libraries(pc_seg ${catkin_LIBRARIES} ${freenect2_LIBRARY} ${ROS_LIBRARIES} ${OpenCV_LIBS}) -target_link_libraries(ft_ex ${PCL_LIBRARIES} ftex ${freenect2_LIBRARY} ${ROS_LIBRARIES} ${OpenCV_LIBS}) +#target_link_libraries(pc_seg ${catkin_LIBRARIES} ${freenect2_LIBRARY} ${catkin_LIBRARIES} ${OpenCV_LIBS}) +target_link_libraries(ft_ex ${PCL_LIBRARIES} ftex ${freenect2_LIBRARY} ${catkin_LIBRARIES} ${OpenCV_LIBS}) add_dependencies(ft_ex hlpr_feature_extraction_generate_messages_cpp) #add_dependencies(pc_seg segmentation_gencfg segmentation_generate_messages_cpp) diff --git a/hlpr_feature_extraction/include/utils.hpp b/hlpr_feature_extraction/include/utils.hpp index ca6bae4..8aac49d 100644 --- a/hlpr_feature_extraction/include/utils.hpp +++ b/hlpr_feature_extraction/include/utils.hpp @@ -349,7 +349,7 @@ array2file(const T *arr, int size, std::ofstream &myfile, char *delim = "\n"); template void -vector2file(const std::vector &vec, char *fileName = "tmp.txt", char *delim = "\n") +vector2file(const std::vector &vec, char *fileName, char *delim) { std::ofstream myfile; myfile.open (fileName); @@ -361,7 +361,7 @@ vector2file(const std::vector &vec, char *fileName = "tmp.txt", char *delim = template void -file2vector(std::vector &vec, char *fileName = "tmp.txt", char delim = ',') +file2vector(std::vector &vec, char *fileName, char delim) { std::ifstream myfile; myfile.open (fileName); @@ -385,7 +385,7 @@ file2vector(std::vector &vec, char *fileName = "tmp.txt", char delim = ',') template void -doubleVector2file(const std::vector > &vec, char *fileName = "tmp.txt", char *delimiter = ",", char *delimiter2 = "\n") +doubleVector2file(const std::vector > &vec, char *fileName, char *delimiter, char *delimiter2) { std::ofstream myfile; myfile.open (fileName); @@ -402,7 +402,7 @@ doubleVector2file(const std::vector > &vec, char *fileName = "tmp template void -file2doubleVector(std::vector > &vec, char *fileName = "tmp.txt", char delimiter = ',', char delimiter2 = '\n') +file2doubleVector(std::vector > &vec, char *fileName, char delimiter, char delimiter2) { std::ifstream myfile; myfile.open (fileName); @@ -446,7 +446,7 @@ file2doubleVector(std::vector > &vec, char *fileName = "tmp.txt", template void -array2file(const T *arr, int size, char *fileName = "tmp.txt", char *delim = "\n") +array2file(const T *arr, int size, char *fileName, char *delim) { std::ofstream myfile; myfile.open (fileName); @@ -457,7 +457,7 @@ array2file(const T *arr, int size, char *fileName = "tmp.txt", char *delim = "\n template void -array2file(const T *arr, int size, std::ofstream &myfile, char *delim = "\n") +array2file(const T *arr, int size, std::ofstream &myfile, char *delim) { for(int i = 0; i < size; i++) myfile << arr[i] << delim; diff --git a/hlpr_nonplanar_feature_extraction/CMakeLists.txt b/hlpr_nonplanar_feature_extraction/CMakeLists.txt index 8793ce0..91889c1 100644 --- a/hlpr_nonplanar_feature_extraction/CMakeLists.txt +++ b/hlpr_nonplanar_feature_extraction/CMakeLists.txt @@ -6,18 +6,17 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native") set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,--as-needed -pthread") set(OPENNI_INCLUDE_DIRS /usr/include/ni) -set(ROS_DISTRO indigo) -set(ROS_LIBRARIES /opt/ros/${ROS_DISTRO}/lib/libroscpp.so - /opt/ros/${ROS_DISTRO}/lib/libroscpp_serialization.so - /opt/ros/${ROS_DISTRO}/lib/librosconsole.so - /opt/ros/${ROS_DISTRO}/lib/libroslib.so - /opt/ros/${ROS_DISTRO}/lib/librostime.so - /opt/ros/${ROS_DISTRO}/lib/libtf2.so - /opt/ros/${ROS_DISTRO}/lib/libtf2_ros.so) #find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure message_generation roscpp std_msgs geometry_msgs) -find_package(catkin REQUIRED COMPONENTS message_generation roscpp std_msgs geometry_msgs tf2_ros hlpr_perception_msgs) -find_package(PCL 1.8 REQUIRED COMPONENTS) +find_package(catkin REQUIRED COMPONENTS + roscpp + message_generation + roscpp + std_msgs geometry_msgs + tf2_ros + hlpr_perception_msgs + pcl_ros +) find_package(freenect2 REQUIRED) find_package(OpenCV 2 REQUIRED) @@ -39,13 +38,13 @@ include_directories(include ${catkin_INCLUDE_DIRS}) include_directories(${PROJECT_SOURCE_DIR}/include) add_library(nonplanarftex SHARED src/cluster_processing.cpp src/rotcalipers.cpp src/utils.cpp src/utils_pcl_ros.cpp) -target_link_libraries(nonplanarftex ${PCL_LIBRARIES} ${ROS_LIBRARIES}) +target_link_libraries(nonplanarftex ${PCL_LIBRARIES} ${catkin_LIBRARIES}) add_dependencies(nonplanarftex hlpr_perception_msgs_generate_messages_cpp) add_executable(nonplanar_ft_ex src/feature_extraction.cpp) #add_executable(listener src/listener.cpp src/nodes/listener_node.cpp) -#target_link_libraries(pc_seg ${catkin_LIBRARIES} ${freenect2_LIBRARY} ${ROS_LIBRARIES} ${OpenCV_LIBS}) -target_link_libraries(nonplanar_ft_ex ${PCL_LIBRARIES} nonplanarftex ${freenect2_LIBRARY} ${ROS_LIBRARIES} ${OpenCV_LIBS}) +#target_link_libraries(pc_seg ${catkin_LIBRARIES} ${freenect2_LIBRARY} ${catkin_LIBRARIES} ${OpenCV_LIBS}) +target_link_libraries(nonplanar_ft_ex ${PCL_LIBRARIES} nonplanarftex ${freenect2_LIBRARY} ${catkin_LIBRARIES} ${OpenCV_LIBS}) add_dependencies(nonplanar_ft_ex hlpr_perception_msgs_generate_messages_cpp) diff --git a/hlpr_nonplanar_feature_extraction/include/utils.hpp b/hlpr_nonplanar_feature_extraction/include/utils.hpp index 7e8645d..18fd43d 100644 --- a/hlpr_nonplanar_feature_extraction/include/utils.hpp +++ b/hlpr_nonplanar_feature_extraction/include/utils.hpp @@ -349,7 +349,7 @@ array2file(const T *arr, int size, std::ofstream &myfile, char *delim = "\n"); template void -vector2file(const std::vector &vec, char *fileName = "tmp.txt", char *delim = "\n") +vector2file(const std::vector &vec, char *fileName, char *delim) { std::ofstream myfile; myfile.open (fileName); @@ -361,7 +361,7 @@ vector2file(const std::vector &vec, char *fileName = "tmp.txt", char *delim = template void -file2vector(std::vector &vec, char *fileName = "tmp.txt", char delim = ',') +file2vector(std::vector &vec, char *fileName, char delim) { std::ifstream myfile; myfile.open (fileName); @@ -385,7 +385,7 @@ file2vector(std::vector &vec, char *fileName = "tmp.txt", char delim = ',') template void -doubleVector2file(const std::vector > &vec, char *fileName = "tmp.txt", char *delimiter = ",", char *delimiter2 = "\n") +doubleVector2file(const std::vector > &vec, char *fileName, char *delimiter, char *delimiter2) { std::ofstream myfile; myfile.open (fileName); @@ -402,7 +402,7 @@ doubleVector2file(const std::vector > &vec, char *fileName = "tmp template void -file2doubleVector(std::vector > &vec, char *fileName = "tmp.txt", char delimiter = ',', char delimiter2 = '\n') +file2doubleVector(std::vector > &vec, char *fileName, char delimiter, char delimiter2) { std::ifstream myfile; myfile.open (fileName); @@ -446,7 +446,7 @@ file2doubleVector(std::vector > &vec, char *fileName = "tmp.txt", template void -array2file(const T *arr, int size, char *fileName = "tmp.txt", char *delim = "\n") +array2file(const T *arr, int size, char *fileName, char *delim) { std::ofstream myfile; myfile.open (fileName); @@ -457,7 +457,7 @@ array2file(const T *arr, int size, char *fileName = "tmp.txt", char *delim = "\n template void -array2file(const T *arr, int size, std::ofstream &myfile, char *delim = "\n") +array2file(const T *arr, int size, std::ofstream &myfile, char *delim) { for(int i = 0; i < size; i++) myfile << arr[i] << delim; diff --git a/hlpr_nonplanar_feature_extraction/package.xml b/hlpr_nonplanar_feature_extraction/package.xml index 1b46666..c8e6a09 100644 --- a/hlpr_nonplanar_feature_extraction/package.xml +++ b/hlpr_nonplanar_feature_extraction/package.xml @@ -15,4 +15,5 @@ tf2_ros std_msgs geometry_msgs + hlpr_perception_msgs diff --git a/hlpr_nonplanar_segmentation/CMakeLists.txt b/hlpr_nonplanar_segmentation/CMakeLists.txt index 80d3ee8..ea68873 100644 --- a/hlpr_nonplanar_segmentation/CMakeLists.txt +++ b/hlpr_nonplanar_segmentation/CMakeLists.txt @@ -6,16 +6,15 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native") set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,--as-needed -pthread") set(OPENNI_INCLUDE_DIRS /usr/include/ni) -set(ROS_DISTRO indigo) -set(ROS_LIBRARIES /opt/ros/${ROS_DISTRO}/lib/libroscpp.so - /opt/ros/${ROS_DISTRO}/lib/libroscpp_serialization.so - /opt/ros/${ROS_DISTRO}/lib/librosconsole.so - /opt/ros/${ROS_DISTRO}/lib/libroslib.so - /opt/ros/${ROS_DISTRO}/lib/librostime.so) - #find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure message_generation roscpp std_msgs geometry_msgs) -find_package(catkin REQUIRED COMPONENTS roscpp std_msgs geometry_msgs sensor_msgs hlpr_perception_msgs) -find_package(PCL 1.8 REQUIRED COMPONENTS) +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs + geometry_msgs + sensor_msgs + hlpr_perception_msgs + pcl_ros +) find_package(freenect2 REQUIRED) find_package(OpenCV 2 REQUIRED) @@ -36,12 +35,12 @@ include_directories(include ${catkin_INCLUDE_DIRS}) include_directories(${PROJECT_SOURCE_DIR}/include) add_library(nonplanarseg SHARED src/pc_segmentation.cpp src/utils.cpp) -target_link_libraries(nonplanarseg ${PCL_LIBRARIES} ${ROS_LIBRARIES}) +target_link_libraries(nonplanarseg ${PCL_LIBRARIES} ${catkin_LIBRARIES}) add_executable(nonplanar_seg src/segmentation.cpp) #add_executable(listener src/listener.cpp src/nodes/listener_node.cpp) -#target_link_libraries(pc_seg ${catkin_LIBRARIES} ${freenect2_LIBRARY} ${ROS_LIBRARIES} ${OpenCV_LIBS}) -target_link_libraries(nonplanar_seg ${PCL_LIBRARIES} nonplanarseg ${freenect2_LIBRARY} ${ROS_LIBRARIES} ${OpenCV_LIBRARIES}) +#target_link_libraries(pc_seg ${catkin_LIBRARIES} ${freenect2_LIBRARY} ${ROS_LIBRARIES} ${catkin_LIBRARIES} ${OpenCV_LIBS}) +target_link_libraries(nonplanar_seg ${PCL_LIBRARIES} nonplanarseg ${freenect2_LIBRARY} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) #add_dependencies(pc_seg segmentation_gencfg segmentation_generate_messages_cpp) add_dependencies(nonplanar_seg hlpr_perception_msgs_generate_messages_cpp) diff --git a/hlpr_nonplanar_segmentation/include/utils.hpp b/hlpr_nonplanar_segmentation/include/utils.hpp index a282e50..89348fd 100644 --- a/hlpr_nonplanar_segmentation/include/utils.hpp +++ b/hlpr_nonplanar_segmentation/include/utils.hpp @@ -394,7 +394,7 @@ array2file(const T *arr, int size, std::ofstream &myfile, char *delim = "\n"); template void -vector2file(const std::vector &vec, char *fileName = "tmp.txt", char *delim = "\n") +vector2file(const std::vector &vec, char *fileName, char *delim) { std::ofstream myfile; myfile.open (fileName); @@ -406,7 +406,7 @@ vector2file(const std::vector &vec, char *fileName = "tmp.txt", char *delim = template void -file2vector(std::vector &vec, char *fileName = "tmp.txt", char delim = ',') +file2vector(std::vector &vec, char *fileName, char delim) { std::ifstream myfile; myfile.open (fileName); @@ -430,7 +430,7 @@ file2vector(std::vector &vec, char *fileName = "tmp.txt", char delim = ',') template void -doubleVector2file(const std::vector > &vec, char *fileName = "tmp.txt", char *delimiter = ",", char *delimiter2 = "\n") +doubleVector2file(const std::vector > &vec, char *fileName, char *delimiter, char *delimiter2) { std::ofstream myfile; myfile.open (fileName); @@ -447,7 +447,7 @@ doubleVector2file(const std::vector > &vec, char *fileName = "tmp template void -file2doubleVector(std::vector > &vec, char *fileName = "tmp.txt", char delimiter = ',', char delimiter2 = '\n') +file2doubleVector(std::vector > &vec, char *fileName, char delimiter, char delimiter2) { std::ifstream myfile; myfile.open (fileName); @@ -491,7 +491,7 @@ file2doubleVector(std::vector > &vec, char *fileName = "tmp.txt", template void -array2file(const T *arr, int size, char *fileName = "tmp.txt", char *delim = "\n") +array2file(const T *arr, int size, char *fileName, char *delim) { std::ofstream myfile; myfile.open (fileName); @@ -502,7 +502,7 @@ array2file(const T *arr, int size, char *fileName = "tmp.txt", char *delim = "\n template void -array2file(const T *arr, int size, std::ofstream &myfile, char *delim = "\n") +array2file(const T *arr, int size, std::ofstream &myfile, char *delim) { for(int i = 0; i < size; i++) myfile << arr[i] << delim; diff --git a/hlpr_nonplanar_segmentation/package.xml b/hlpr_nonplanar_segmentation/package.xml index fd4f40a..febae10 100644 --- a/hlpr_nonplanar_segmentation/package.xml +++ b/hlpr_nonplanar_segmentation/package.xml @@ -15,4 +15,5 @@ std_msgs geometry_msgs sensor_msgs + hlpr_perception_msgs diff --git a/hlpr_perception_msgs/CMakeLists.txt b/hlpr_perception_msgs/CMakeLists.txt index c571951..9565b1b 100644 --- a/hlpr_perception_msgs/CMakeLists.txt +++ b/hlpr_perception_msgs/CMakeLists.txt @@ -1,15 +1,15 @@ cmake_minimum_required(VERSION 2.8.3) project(hlpr_perception_msgs) find_package(catkin REQUIRED COMPONENTS + roscpp geometry_msgs sensor_msgs std_msgs tf2_ros message_generation + pcl_ros ) -find_package(PCL 1.8 REQUIRED COMPONENTS) - add_message_files( FILES # Segmentation msgs @@ -17,7 +17,7 @@ add_message_files( SegClusters.msg SegPlanes.msg ClusterIndex.msg - # Feature extraction msgs + # Feature extraction msgsnrg BasicFeatures.msg ShapeHist.msg ColorHist.msg diff --git a/hlpr_segmentation/CMakeLists.txt b/hlpr_segmentation/CMakeLists.txt index 91f08c9..828c806 100644 --- a/hlpr_segmentation/CMakeLists.txt +++ b/hlpr_segmentation/CMakeLists.txt @@ -6,15 +6,17 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native") set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,--as-needed -pthread") set(OPENNI_INCLUDE_DIRS /usr/include/ni) -set(ROS_DISTRO indigo) -set(ROS_LIBRARIES /opt/ros/${ROS_DISTRO}/lib/libroscpp.so - /opt/ros/${ROS_DISTRO}/lib/libroscpp_serialization.so - /opt/ros/${ROS_DISTRO}/lib/librosconsole.so - /opt/ros/${ROS_DISTRO}/lib/libroslib.so - /opt/ros/${ROS_DISTRO}/lib/librostime.so) #find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure message_generation roscpp std_msgs geometry_msgs) -find_package(catkin REQUIRED COMPONENTS message_generation roscpp std_msgs geometry_msgs sensor_msgs) +find_package(catkin REQUIRED COMPONENTS + message_generation + roscpp + std_msgs + geometry_msgs + sensor_msgs + roscpp + hlpr_perception_msgs +) find_package(OpenCL QUIET) IF(OpenCL_FOUND) SET(LIBRARIES @@ -63,8 +65,8 @@ target_link_libraries(pcseg ${PCL_LIBRARIES} ${ROS_LIBRARIES}) add_executable(pc_seg src/segmentation.cpp) #add_executable(listener src/listener.cpp src/nodes/listener_node.cpp) -#target_link_libraries(pc_seg ${catkin_LIBRARIES} ${freenect2_LIBRARY} ${ROS_LIBRARIES} ${OpenCV_LIBS}) -target_link_libraries(pc_seg ${PCL_LIBRARIES} pcseg ${freenect2_LIBRARY} ${ROS_LIBRARIES} ${OpenCV_LIBS}) +#target_link_libraries(pc_seg ${catkin_LIBRARIES} ${freenect2_LIBRARY} ${catkin_LIBRARIES} ${OpenCV_LIBS}) +target_link_libraries(pc_seg ${PCL_LIBRARIES} pcseg ${freenect2_LIBRARY} ${catkin_LIBRARIES} ${OpenCV_LIBS}) #add_dependencies(pc_seg segmentation_gencfg segmentation_generate_messages_cpp) add_dependencies(pc_seg segmentation_generate_messages_cpp hlpr_perception_msgs_generate_messages_cpp) diff --git a/hlpr_segmentation/include/utils.hpp b/hlpr_segmentation/include/utils.hpp index ca6bae4..8aac49d 100644 --- a/hlpr_segmentation/include/utils.hpp +++ b/hlpr_segmentation/include/utils.hpp @@ -349,7 +349,7 @@ array2file(const T *arr, int size, std::ofstream &myfile, char *delim = "\n"); template void -vector2file(const std::vector &vec, char *fileName = "tmp.txt", char *delim = "\n") +vector2file(const std::vector &vec, char *fileName, char *delim) { std::ofstream myfile; myfile.open (fileName); @@ -361,7 +361,7 @@ vector2file(const std::vector &vec, char *fileName = "tmp.txt", char *delim = template void -file2vector(std::vector &vec, char *fileName = "tmp.txt", char delim = ',') +file2vector(std::vector &vec, char *fileName, char delim) { std::ifstream myfile; myfile.open (fileName); @@ -385,7 +385,7 @@ file2vector(std::vector &vec, char *fileName = "tmp.txt", char delim = ',') template void -doubleVector2file(const std::vector > &vec, char *fileName = "tmp.txt", char *delimiter = ",", char *delimiter2 = "\n") +doubleVector2file(const std::vector > &vec, char *fileName, char *delimiter, char *delimiter2) { std::ofstream myfile; myfile.open (fileName); @@ -402,7 +402,7 @@ doubleVector2file(const std::vector > &vec, char *fileName = "tmp template void -file2doubleVector(std::vector > &vec, char *fileName = "tmp.txt", char delimiter = ',', char delimiter2 = '\n') +file2doubleVector(std::vector > &vec, char *fileName, char delimiter, char delimiter2) { std::ifstream myfile; myfile.open (fileName); @@ -446,7 +446,7 @@ file2doubleVector(std::vector > &vec, char *fileName = "tmp.txt", template void -array2file(const T *arr, int size, char *fileName = "tmp.txt", char *delim = "\n") +array2file(const T *arr, int size, char *fileName, char *delim) { std::ofstream myfile; myfile.open (fileName); @@ -457,7 +457,7 @@ array2file(const T *arr, int size, char *fileName = "tmp.txt", char *delim = "\n template void -array2file(const T *arr, int size, std::ofstream &myfile, char *delim = "\n") +array2file(const T *arr, int size, std::ofstream &myfile, char *delim) { for(int i = 0; i < size; i++) myfile << arr[i] << delim; diff --git a/hlpr_single_plane_segmentation/CMakeLists.txt b/hlpr_single_plane_segmentation/CMakeLists.txt index 59ea9f3..a5de04f 100644 --- a/hlpr_single_plane_segmentation/CMakeLists.txt +++ b/hlpr_single_plane_segmentation/CMakeLists.txt @@ -14,8 +14,15 @@ set(ROS_LIBRARIES /opt/ros/${ROS_DISTRO}/lib/libroscpp.so /opt/ros/${ROS_DISTRO}/lib/librostime.so) #find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure message_generation roscpp std_msgs geometry_msgs) -find_package(catkin REQUIRED COMPONENTS message_generation roscpp std_msgs geometry_msgs sensor_msgs hlpr_perception_msgs) -find_package(PCL 1.8 REQUIRED COMPONENTS) +find_package(catkin REQUIRED COMPONENTS + message_generation + roscpp + std_msgs + geometry_msgs + sensor_msgs + hlpr_perception_msgs + pcl_ros +) find_package(freenect2 REQUIRED) find_package(OpenCV 2 REQUIRED) @@ -50,10 +57,10 @@ include_directories(include ${catkin_INCLUDE_DIRS}) include_directories(${PROJECT_SOURCE_DIR}/include) add_library(spseg SHARED src/pc_segmentation.cpp src/utils.cpp) -target_link_libraries(spseg ${PCL_LIBRARIES} ${ROS_LIBRARIES}) +target_link_libraries(spseg ${PCL_LIBRARIES} ${catkin_LIBRARIES}) add_executable(sp_seg src/segmentation.cpp) -target_link_libraries(sp_seg ${PCL_LIBRARIES} spseg ${freenect2_LIBRARY} ${ROS_LIBRARIES} ${OpenCV_LIBS}) +target_link_libraries(sp_seg ${PCL_LIBRARIES} spseg ${freenect2_LIBRARY} ${catkin_LIBRARIES} ${OpenCV_LIBS}) add_dependencies(sp_seg hlpr_perception_msgs_generate_messages_cpp) diff --git a/hlpr_single_plane_segmentation/include/utils.hpp b/hlpr_single_plane_segmentation/include/utils.hpp index ca6bae4..8aac49d 100644 --- a/hlpr_single_plane_segmentation/include/utils.hpp +++ b/hlpr_single_plane_segmentation/include/utils.hpp @@ -349,7 +349,7 @@ array2file(const T *arr, int size, std::ofstream &myfile, char *delim = "\n"); template void -vector2file(const std::vector &vec, char *fileName = "tmp.txt", char *delim = "\n") +vector2file(const std::vector &vec, char *fileName, char *delim) { std::ofstream myfile; myfile.open (fileName); @@ -361,7 +361,7 @@ vector2file(const std::vector &vec, char *fileName = "tmp.txt", char *delim = template void -file2vector(std::vector &vec, char *fileName = "tmp.txt", char delim = ',') +file2vector(std::vector &vec, char *fileName, char delim) { std::ifstream myfile; myfile.open (fileName); @@ -385,7 +385,7 @@ file2vector(std::vector &vec, char *fileName = "tmp.txt", char delim = ',') template void -doubleVector2file(const std::vector > &vec, char *fileName = "tmp.txt", char *delimiter = ",", char *delimiter2 = "\n") +doubleVector2file(const std::vector > &vec, char *fileName, char *delimiter, char *delimiter2) { std::ofstream myfile; myfile.open (fileName); @@ -402,7 +402,7 @@ doubleVector2file(const std::vector > &vec, char *fileName = "tmp template void -file2doubleVector(std::vector > &vec, char *fileName = "tmp.txt", char delimiter = ',', char delimiter2 = '\n') +file2doubleVector(std::vector > &vec, char *fileName, char delimiter, char delimiter2) { std::ifstream myfile; myfile.open (fileName); @@ -446,7 +446,7 @@ file2doubleVector(std::vector > &vec, char *fileName = "tmp.txt", template void -array2file(const T *arr, int size, char *fileName = "tmp.txt", char *delim = "\n") +array2file(const T *arr, int size, char *fileName, char *delim) { std::ofstream myfile; myfile.open (fileName); @@ -457,7 +457,7 @@ array2file(const T *arr, int size, char *fileName = "tmp.txt", char *delim = "\n template void -array2file(const T *arr, int size, std::ofstream &myfile, char *delim = "\n") +array2file(const T *arr, int size, std::ofstream &myfile, char *delim) { for(int i = 0; i < size; i++) myfile << arr[i] << delim; From 2ea2595167d0592ef5ffc382a1a3ca9b24ed338f Mon Sep 17 00:00:00 2001 From: vector Date: Mon, 12 Feb 2018 15:01:32 -0600 Subject: [PATCH 13/28] Update CMakeLists.txt to OpenCV 3 --- hlpr_feature_extraction/CMakeLists.txt | 2 +- hlpr_nonplanar_feature_extraction/CMakeLists.txt | 2 +- hlpr_nonplanar_segmentation/CMakeLists.txt | 2 +- hlpr_segmentation/CMakeLists.txt | 5 +++-- hlpr_single_plane_segmentation/CMakeLists.txt | 2 +- 5 files changed, 7 insertions(+), 6 deletions(-) diff --git a/hlpr_feature_extraction/CMakeLists.txt b/hlpr_feature_extraction/CMakeLists.txt index 19fa287..8a0201d 100644 --- a/hlpr_feature_extraction/CMakeLists.txt +++ b/hlpr_feature_extraction/CMakeLists.txt @@ -25,7 +25,7 @@ find_package(catkin REQUIRED COMPONENTS pcl_ros ) find_package(freenect2 REQUIRED) -find_package(OpenCV 2 REQUIRED) +find_package(OpenCV 3 REQUIRED) add_message_files( FILES diff --git a/hlpr_nonplanar_feature_extraction/CMakeLists.txt b/hlpr_nonplanar_feature_extraction/CMakeLists.txt index 91889c1..7b3ccf2 100644 --- a/hlpr_nonplanar_feature_extraction/CMakeLists.txt +++ b/hlpr_nonplanar_feature_extraction/CMakeLists.txt @@ -18,7 +18,7 @@ find_package(catkin REQUIRED COMPONENTS pcl_ros ) find_package(freenect2 REQUIRED) -find_package(OpenCV 2 REQUIRED) +find_package(OpenCV 3 REQUIRED) #generate_dynamic_reconfigure_options( # cfg/segmentation.cfg diff --git a/hlpr_nonplanar_segmentation/CMakeLists.txt b/hlpr_nonplanar_segmentation/CMakeLists.txt index ea68873..791ee71 100644 --- a/hlpr_nonplanar_segmentation/CMakeLists.txt +++ b/hlpr_nonplanar_segmentation/CMakeLists.txt @@ -16,7 +16,7 @@ find_package(catkin REQUIRED COMPONENTS pcl_ros ) find_package(freenect2 REQUIRED) -find_package(OpenCV 2 REQUIRED) +find_package(OpenCV 3 REQUIRED) #generate_dynamic_reconfigure_options( # cfg/segmentation.cfg diff --git a/hlpr_segmentation/CMakeLists.txt b/hlpr_segmentation/CMakeLists.txt index 828c806..a380811 100644 --- a/hlpr_segmentation/CMakeLists.txt +++ b/hlpr_segmentation/CMakeLists.txt @@ -25,9 +25,10 @@ IF(OpenCL_FOUND) ) ENDIF(OpenCL_FOUND) -find_package(PCL 1.7 REQUIRED COMPONENTS) +#find_package(PCL 1.8 REQUIRED COMPONENTS) +find_package(PCL REQUIRED) find_package(freenect2 REQUIRED) -find_package(OpenCV 2 REQUIRED) +find_package(OpenCV 3 REQUIRED) add_message_files( FILES diff --git a/hlpr_single_plane_segmentation/CMakeLists.txt b/hlpr_single_plane_segmentation/CMakeLists.txt index a5de04f..f3ed801 100644 --- a/hlpr_single_plane_segmentation/CMakeLists.txt +++ b/hlpr_single_plane_segmentation/CMakeLists.txt @@ -24,7 +24,7 @@ find_package(catkin REQUIRED COMPONENTS pcl_ros ) find_package(freenect2 REQUIRED) -find_package(OpenCV 2 REQUIRED) +find_package(OpenCV 3 REQUIRED) #add_message_files( # FILES From ca15fbcb6780e692b3cc7c19fe76dfeb01d61c29 Mon Sep 17 00:00:00 2001 From: vector Date: Mon, 12 Feb 2018 15:02:14 -0600 Subject: [PATCH 14/28] Resove eigen align issue --- hlpr_nonplanar_segmentation/include/k2g.h | 4 ++-- hlpr_segmentation/include/k2g.h | 4 ++-- hlpr_single_plane_segmentation/include/k2g.h | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/hlpr_nonplanar_segmentation/include/k2g.h b/hlpr_nonplanar_segmentation/include/k2g.h index 5fecec4..97665ea 100644 --- a/hlpr_nonplanar_segmentation/include/k2g.h +++ b/hlpr_nonplanar_segmentation/include/k2g.h @@ -248,8 +248,8 @@ class K2G { libfreenect2::SyncMultiFrameListener listener_; libfreenect2::FrameMap frames_; libfreenect2::Frame undistorted_, registered_, big_mat_; - Eigen::Matrix colmap; - Eigen::Matrix rowmap; + Eigen::Matrix colmap; + Eigen::Matrix rowmap; std::string serial_; int map_[512 * 424]; float qnan_; diff --git a/hlpr_segmentation/include/k2g.h b/hlpr_segmentation/include/k2g.h index 5fecec4..97665ea 100644 --- a/hlpr_segmentation/include/k2g.h +++ b/hlpr_segmentation/include/k2g.h @@ -248,8 +248,8 @@ class K2G { libfreenect2::SyncMultiFrameListener listener_; libfreenect2::FrameMap frames_; libfreenect2::Frame undistorted_, registered_, big_mat_; - Eigen::Matrix colmap; - Eigen::Matrix rowmap; + Eigen::Matrix colmap; + Eigen::Matrix rowmap; std::string serial_; int map_[512 * 424]; float qnan_; diff --git a/hlpr_single_plane_segmentation/include/k2g.h b/hlpr_single_plane_segmentation/include/k2g.h index 5fecec4..97665ea 100644 --- a/hlpr_single_plane_segmentation/include/k2g.h +++ b/hlpr_single_plane_segmentation/include/k2g.h @@ -248,8 +248,8 @@ class K2G { libfreenect2::SyncMultiFrameListener listener_; libfreenect2::FrameMap frames_; libfreenect2::Frame undistorted_, registered_, big_mat_; - Eigen::Matrix colmap; - Eigen::Matrix rowmap; + Eigen::Matrix colmap; + Eigen::Matrix rowmap; std::string serial_; int map_[512 * 424]; float qnan_; From bcea7dc6e955082c5a1a63a6f6a1fd1a324b755f Mon Sep 17 00:00:00 2001 From: Elaine S Date: Mon, 2 Apr 2018 10:41:22 -0500 Subject: [PATCH 15/28] adding low-level audio and visual feature extraction --- hlpr_audio_features/CMakeLists.txt | 199 ++++++++++++++++++ hlpr_audio_features/package.xml | 68 ++++++ hlpr_audio_features/setup.py | 12 ++ .../src/hlpr_audio_features/__init__.py | 0 .../src/hlpr_audio_features/audio_features.py | 154 ++++++++++++++ hlpr_visual_features/CMakeLists.txt | 199 ++++++++++++++++++ hlpr_visual_features/package.xml | 68 ++++++ hlpr_visual_features/setup.py | 12 ++ .../src/hlpr_visual_features/__init__.py | 0 .../hlpr_visual_features/open_cv_features.py | 134 ++++++++++++ 10 files changed, 846 insertions(+) create mode 100644 hlpr_audio_features/CMakeLists.txt create mode 100644 hlpr_audio_features/package.xml create mode 100644 hlpr_audio_features/setup.py create mode 100644 hlpr_audio_features/src/hlpr_audio_features/__init__.py create mode 100755 hlpr_audio_features/src/hlpr_audio_features/audio_features.py create mode 100644 hlpr_visual_features/CMakeLists.txt create mode 100644 hlpr_visual_features/package.xml create mode 100644 hlpr_visual_features/setup.py create mode 100644 hlpr_visual_features/src/hlpr_visual_features/__init__.py create mode 100755 hlpr_visual_features/src/hlpr_visual_features/open_cv_features.py diff --git a/hlpr_audio_features/CMakeLists.txt b/hlpr_audio_features/CMakeLists.txt new file mode 100644 index 0000000..26b0ebd --- /dev/null +++ b/hlpr_audio_features/CMakeLists.txt @@ -0,0 +1,199 @@ +cmake_minimum_required(VERSION 2.8.3) +project(hlpr_audio_features) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES hlpr_audio_features +# CATKIN_DEPENDS roscpp rospy std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/hlpr_audio_features.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/hlpr_audio_features_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_hlpr_audio_features.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/hlpr_audio_features/package.xml b/hlpr_audio_features/package.xml new file mode 100644 index 0000000..4209e7f --- /dev/null +++ b/hlpr_audio_features/package.xml @@ -0,0 +1,68 @@ + + + hlpr_audio_features + 0.0.0 + The hlpr_audio_features package + + + + + eshort + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + + + + + + + + diff --git a/hlpr_audio_features/setup.py b/hlpr_audio_features/setup.py new file mode 100644 index 0000000..be337e5 --- /dev/null +++ b/hlpr_audio_features/setup.py @@ -0,0 +1,12 @@ +## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + packages=['hlpr_audio_features'], + package_dir={'': 'src'}, +) + +setup(**setup_args) diff --git a/hlpr_audio_features/src/hlpr_audio_features/__init__.py b/hlpr_audio_features/src/hlpr_audio_features/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/hlpr_audio_features/src/hlpr_audio_features/audio_features.py b/hlpr_audio_features/src/hlpr_audio_features/audio_features.py new file mode 100755 index 0000000..da92b66 --- /dev/null +++ b/hlpr_audio_features/src/hlpr_audio_features/audio_features.py @@ -0,0 +1,154 @@ +#!/usr/bin/env python + +# Copyright (c) 2017, Elaine Short, SIM Lab +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the SIM Lab nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +import rospy +import pyaudio as pya +from std_msgs.msg import Float64MultiArray +import numpy as np +from scipy import stats +import struct +import threading +import argparse + +VERBOSE=False + +class AudioFeature(): + def __init__(self, print_only): + self._pa = pya.PyAudio() + d_info=None + d_index=None + + self._channels = 4 + #self._rate = 48000 + self._rate = 16000 + fmt = pya.paInt16 + self._timestep = 0.01 + self._frames_per_block = int(self._timestep*self._rate) + self._data = {} + self._data["data"]=[] + + found = False + + for i in range(self._pa.get_device_count()): + d_info = self._pa.get_device_info_by_index(i) + name = d_info["name"] + channels = d_info["maxInputChannels"] + if channels >0: + print i, ":", d_info["name"], channels + + if not found or print_only: + exit(-1) + + self._stream = self._pa.open(format=fmt, channels=self._channels, rate=self._rate, input=True, frames_per_buffer=self._frames_per_block, input_device_index=d_index) + self._intensity_pub = rospy.Publisher("/contingency/audio/intensity",ArrayFeature, queue_size=1) + self._flatness_pub = rospy.Publisher("/contingency/audio/flatness", ArrayFeature, queue_size=1) + self._band_pub = rospy.Publisher("/contingency/audio/bands", ArrayFeature, queue_size=1) + + + self._listen_thread=threading.Thread(target=self.listen) + self._listen_thread.start() + + def get_features(self,data): + data = np.reshape(data, [4,-1],"F") + #print data + band_features = [[] for i in range(5)] + flatness_features = [] + amp_features = [] + for k in range(4): + fft = np.fft.fft(data[k]) + spectrum = abs(fft)**2 + freqs = abs(np.fft.fftfreq(len(data[k]),d=1.0/self._rate)) + + bands = [0,300,1200,2100,3000,24000] + + band_energies = [0 for b in range(len(bands)-1)] + total_energy = sum(spectrum) + + for j in range(len(freqs)): + for i in range(len(bands)-1): + if freqs[j]>=bands[i] and freqs[j] + + hlpr_visual_features + 0.0.0 + The hlpr_visual_features package + + + + + eshort + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + + + + + + + + diff --git a/hlpr_visual_features/setup.py b/hlpr_visual_features/setup.py new file mode 100644 index 0000000..f3f746d --- /dev/null +++ b/hlpr_visual_features/setup.py @@ -0,0 +1,12 @@ +## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + packages=['hlpr_visual_features'], + package_dir={'': 'src'}, +) + +setup(**setup_args) diff --git a/hlpr_visual_features/src/hlpr_visual_features/__init__.py b/hlpr_visual_features/src/hlpr_visual_features/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/hlpr_visual_features/src/hlpr_visual_features/open_cv_features.py b/hlpr_visual_features/src/hlpr_visual_features/open_cv_features.py new file mode 100755 index 0000000..aa83e22 --- /dev/null +++ b/hlpr_visual_features/src/hlpr_visual_features/open_cv_features.py @@ -0,0 +1,134 @@ +#!/usr/bin/env python + +# Copyright (c) 2017, Elaine Short, SIM Lab +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the SIM Lab nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + +import rospy +import cv2 +import numpy +import math +import argparse +from cv_bridge import CvBridge, CvBridgeError +from sensor_msgs.msg import Image +from contingency.msg import ArrayFeature + + +class CVImageSubscriber: + def __init__(self, display_on): + self._sub=rospy.Subscriber("/camera/rgb/image_rect_color", Image, self._cb) + self._bridge = CvBridge() + self._fgbg = cv2.createBackgroundSubtractorMOG2(history=20) + self._motion_pub = rospy.Publisher("/contingency/opencv/motion",ArrayFeature, queue_size=1) + + self._flow_ang_pub = rospy.Publisher("/contingency/opencv/flow/angle",ArrayFeature, queue_size=1) + self._mag_pub = rospy.Publisher("/contingency/opencv/flow/magnitude",ArrayFeature, queue_size=1) + self._acc_pub = rospy.Publisher("/contingency/opencv/flow/acceleration",ArrayFeature, queue_size=1) + self._mag_total_pub = rospy.Publisher("/contingency/opencv/flow/total/magnitude",ArrayFeature, queue_size=1) + self._ang_total_pub = rospy.Publisher("/contingency/opencv/flow/total/angle",ArrayFeature, queue_size=1) + + + self._splits = 4 + self._disp = display_on + self._prev = [] + self._prev_mag = [0 for i in range(self._splits)] + + def _cb(self,data): + try: + cv_image=self._bridge.imgmsg_to_cv2(data, "passthrough") + cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) + except CvBridgeError as e: + rospy.logerr(e) + return + + fgmask=self._fgbg.apply(cv_image) + kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,(3,3)) + fgmask = cv2.morphologyEx(fgmask,cv2.MORPH_OPEN, kernel) + + flow = [] + if len(self._prev)>0: + hsv = numpy.zeros_like(cv2.cvtColor(cv2.cvtColor(cv_image,cv2.COLOR_GRAY2BGR),cv2.COLOR_BGR2HSV)) + hsv[...,1] = 255 + flow = cv2.calcOpticalFlowFarneback(self._prev,cv_image, None, + 0.5, 3, 15, 3, 5, 1.2, 0) + mag, ang = cv2.cartToPolar(flow[...,0], flow[...,1]) + total = [numpy.sum(flow[...,0]), + numpy.sum(flow[...,1])] + + total_mag = math.sqrt(total[0]*total[0]+total[1]*total[1]) + total_ang = math.atan2(total[1],total[0]) + + mag[numpy.isinf(mag)]=0 + + if self._disp: + cv2.imshow("frame",fgmask)#cv2.bitwise_and(cv_image,cv_image,mask=fgmask)) + cv2.imshow("frame0",cv_image) + if len(flow)>0: + hsv[...,0] = ang*180/numpy.pi/2 + hsv[...,2] = cv2.normalize(mag,None,0,255,cv2.NORM_MINMAX) + bgr = cv2.cvtColor(hsv,cv2.COLOR_HSV2BGR) + cv2.imshow('frame2',bgr) + cv2.waitKey(3) + + w = int(fgmask.shape[1]/self._splits) + + data_movement = [] + data_flow_mag = [] + data_flow_ang = [] + + for i in range(self._splits): + start = i*w + end = min(i*w, fgmask.shape[1]) + data_movement.append(numpy.average(fgmask[:,w*i:w*(i+1)])) + if len(flow)>0: + data_flow_mag.append(numpy.average(mag[:,w*i:w*(i+1)])) + try: + data_flow_ang.append(numpy.average(ang[:,w*i:w*(i+1)],weights = mag[:,w*i:w*(i+1)]*mag[:,w*i:w*(i+1)]>10)) + except ZeroDivisionError: + data_flow_ang.append(0) + self._prev = cv_image + + + self._motion_pub.publish(ArrayFeature(data=data_movement)) + + if len(flow)>0: + self._flow_ang_pub.publish(ArrayFeature(data=data_flow_ang)) + self._mag_pub.publish(ArrayFeature(data=data_flow_mag)) + self._acc_pub.publish(ArrayFeature(data=[data_flow_mag[i]-self._prev_mag[i] for i in range(len(data_flow_mag))])) + self._mag_total_pub.publish(ArrayFeature(data=[total_mag])) + self._ang_total_pub.publish(ArrayFeature(data=[total_ang])) + self._prev_mag = data_flow_mag + +if __name__=="__main__": + rospy.init_node("background_subtraction_features") + parser=argparse.ArgumentParser(description="Use opencv to get motion features from video") + parser.add_argument('-d', '--display-video', help="Show the masked video on screen", action='store_true') + args = parser.parse_known_args()[0] + + c = CVImageSubscriber(args.display_video) + rospy.spin() From 479ff7a503707109f061ade8c194fdb09c72892d Mon Sep 17 00:00:00 2001 From: Akanksha Saran Date: Fri, 4 May 2018 23:52:30 -0500 Subject: [PATCH 16/28] Detect multiple faces on an image topic --- hlpr_face_detection/CMakeLists.txt | 206 ++++++++++++++++++ .../launch/face_detector.launch | 10 + .../msg/FaceDetectionTopic.msg | 1 + hlpr_face_detection/package.xml | 74 +++++++ hlpr_face_detection/setup.py | 12 + hlpr_face_detection/src/__init__.py | 0 hlpr_face_detection/src/face_detector.py | 53 +++++ 7 files changed, 356 insertions(+) create mode 100644 hlpr_face_detection/CMakeLists.txt create mode 100644 hlpr_face_detection/launch/face_detector.launch create mode 100644 hlpr_face_detection/msg/FaceDetectionTopic.msg create mode 100644 hlpr_face_detection/package.xml create mode 100644 hlpr_face_detection/setup.py create mode 100644 hlpr_face_detection/src/__init__.py create mode 100755 hlpr_face_detection/src/face_detector.py diff --git a/hlpr_face_detection/CMakeLists.txt b/hlpr_face_detection/CMakeLists.txt new file mode 100644 index 0000000..c98daf3 --- /dev/null +++ b/hlpr_face_detection/CMakeLists.txt @@ -0,0 +1,206 @@ +cmake_minimum_required(VERSION 2.8.3) +project(face_detection) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + message_generation + rospy + cv_bridge + sensor_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +add_message_files( + FILES + FaceDetectionTopic.msg +# Message2.msg +) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs # Or other packages containing msgs + sensor_msgs +) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES face_detection +# CATKIN_DEPENDS message_generation rospy +# DEPENDS system_lib + CATKIN_DEPENDS + cv_bridge + image_transport + message_runtime + sensor_msgs +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/face_detection.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/face_detection_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark executables and/or libraries for installation +# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_face_detection.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/hlpr_face_detection/launch/face_detector.launch b/hlpr_face_detection/launch/face_detector.launch new file mode 100644 index 0000000..4b91fc1 --- /dev/null +++ b/hlpr_face_detection/launch/face_detector.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/hlpr_face_detection/msg/FaceDetectionTopic.msg b/hlpr_face_detection/msg/FaceDetectionTopic.msg new file mode 100644 index 0000000..5794032 --- /dev/null +++ b/hlpr_face_detection/msg/FaceDetectionTopic.msg @@ -0,0 +1 @@ +std_msgs/Float32MultiArray[] faces diff --git a/hlpr_face_detection/package.xml b/hlpr_face_detection/package.xml new file mode 100644 index 0000000..c3929de --- /dev/null +++ b/hlpr_face_detection/package.xml @@ -0,0 +1,74 @@ + + + face_detection + 1.0.0 + The face_detection package + + + + + Akanksha Saran + Akanksha Saran + + + + + + BSD + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + message_generation + rospy + std_msgs + cv_bridge + image_transport + sensor_msgs + + rospy + + rospy + std_msgs + cv_bridge + image_transport + message_runtime + sensor_msgs + + + + + + + diff --git a/hlpr_face_detection/setup.py b/hlpr_face_detection/setup.py new file mode 100644 index 0000000..4d6cfda --- /dev/null +++ b/hlpr_face_detection/setup.py @@ -0,0 +1,12 @@ +## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + packages=['face_detection'], + package_dir={'': 'src'}, +) + +setup(**setup_args) diff --git a/hlpr_face_detection/src/__init__.py b/hlpr_face_detection/src/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/hlpr_face_detection/src/face_detector.py b/hlpr_face_detection/src/face_detector.py new file mode 100755 index 0000000..9b41ef0 --- /dev/null +++ b/hlpr_face_detection/src/face_detector.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python +import rospy +import sys +import cv2 +from std_msgs.msg import String, Float32MultiArray, Bool +from cv_bridge import CvBridge, CvBridgeError +from sensor_msgs.msg import Image, PointCloud2, CompressedImage +from face_detection.msg import FaceDetectionTopic +import face_recognition + +class FaceDetector: + def __init__(self, tgtdir='.'): + print "Starting Init for Face Detector" + self.face_detector_pub = rospy.Publisher("face_detections",FaceDetectionTopic,queue_size=10) + # rospy.sleep(2) + self.bridge = CvBridge() + self.rgb_image = None + self.tgtdir = tgtdir + image_topic = rospy.get_param("/image_topic_name") + detection_sub = rospy.Subscriber(image_topic,Image, self.callback, queue_size=1, buff_size=52428800) + + rospy.sleep(5) + print "Finished Init for Face Detector" + + + def callback(self,rgb): + self.rgb_image = rgb + print "callback" + cv_image = self.bridge.imgmsg_to_cv2(rgb, "bgr8") + + faces = self.detect_faces(cv_image) + msg = FaceDetectionTopic() + msg.faces = faces + # print "faces {0}".format(faces) + self.face_detector_pub.publish(msg) + + + def detect_faces(self, img): + faces = face_recognition.face_locations(img, number_of_times_to_upsample=1) + multi_array = [] + for face in faces: + face_array = Float32MultiArray() + face_array.data = face + multi_array.append(face_array) + + return multi_array + +if __name__ == '__main__': + rospy.init_node('face_detector', anonymous=True) + obj = FaceDetector() + while not rospy.is_shutdown(): + rospy.spin() + From 7d80e7b33957a929bd2929986b10f54488e478d3 Mon Sep 17 00:00:00 2001 From: Akanksha Saran Date: Sat, 5 May 2018 00:01:26 -0500 Subject: [PATCH 17/28] Added README --- hlpr_face_detection/README.md | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) create mode 100644 hlpr_face_detection/README.md diff --git a/hlpr_face_detection/README.md b/hlpr_face_detection/README.md new file mode 100644 index 0000000..45d0fbe --- /dev/null +++ b/hlpr_face_detection/README.md @@ -0,0 +1,28 @@ +# Install dlib from source (without GPU support/CUDA) +```` +$ cd ~/ +$ git clone https://github.com/davisking/dlib.git +$ cd dlib +$ mkdir build; cd build; cmake .. -DDLIB_USE_CUDA=0; cmake --build . +$ cd .. +$ sudo python setup.py install --no USE_AVX_INSTRUCTIONS --no DLIB_USE_CUDA +```` + +#Install dlib from source (with GPU support/CUDA) +```` +$ cd ~/ +$ git clone https://github.com/davisking/dlib.git +$ cd dlib +$ mkdir build; cd build; cmake .. -DDLIB_USE_CUDA=1; cmake --build . +$ cd .. +$ sudo python setup.py install --no USE_AVX_INSTRUCTIONS +```` + +# Install the face recognition package +```` +sudo pip install --upgrade scipy +sudo pip install face_recognition +sudo pip install git+https://github.com/ageitgey/face_recognition_models +```` + +Check `https://github.com/ageitgey/face_recognition#face-recognition` for more details. From 97798e4a3e0983d6b80092312bd86cf51f99eef6 Mon Sep 17 00:00:00 2001 From: Akanksha Saran Date: Sat, 5 May 2018 00:02:11 -0500 Subject: [PATCH 18/28] Fixed README --- hlpr_face_detection/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hlpr_face_detection/README.md b/hlpr_face_detection/README.md index 45d0fbe..f8b7d92 100644 --- a/hlpr_face_detection/README.md +++ b/hlpr_face_detection/README.md @@ -8,7 +8,7 @@ $ cd .. $ sudo python setup.py install --no USE_AVX_INSTRUCTIONS --no DLIB_USE_CUDA ```` -#Install dlib from source (with GPU support/CUDA) +# Install dlib from source (with GPU support/CUDA) ```` $ cd ~/ $ git clone https://github.com/davisking/dlib.git From 862e3f7605bfe1a113eed2726c556f3946085ec4 Mon Sep 17 00:00:00 2001 From: Akanksha Saran Date: Sat, 5 May 2018 00:03:49 -0500 Subject: [PATCH 19/28] Fixed README --- hlpr_face_detection/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hlpr_face_detection/README.md b/hlpr_face_detection/README.md index f8b7d92..9357359 100644 --- a/hlpr_face_detection/README.md +++ b/hlpr_face_detection/README.md @@ -25,4 +25,4 @@ sudo pip install face_recognition sudo pip install git+https://github.com/ageitgey/face_recognition_models ```` -Check `https://github.com/ageitgey/face_recognition#face-recognition` for more details. +Check for more details. From 9b4fda574841db54bf39a05451f585fe2e547dc2 Mon Sep 17 00:00:00 2001 From: Akanksha Saran Date: Sat, 5 May 2018 00:12:51 -0500 Subject: [PATCH 20/28] Changes for new package name --- hlpr_face_detection/CMakeLists.txt | 2 +- hlpr_face_detection/launch/face_detector.launch | 2 +- hlpr_face_detection/package.xml | 2 +- hlpr_face_detection/setup.py | 2 +- hlpr_face_detection/src/face_detector.py | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/hlpr_face_detection/CMakeLists.txt b/hlpr_face_detection/CMakeLists.txt index c98daf3..5dd652d 100644 --- a/hlpr_face_detection/CMakeLists.txt +++ b/hlpr_face_detection/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(face_detection) +project(hlpr_face_detection) ## Compile as C++11, supported in ROS Kinetic and newer # add_compile_options(-std=c++11) diff --git a/hlpr_face_detection/launch/face_detector.launch b/hlpr_face_detection/launch/face_detector.launch index 4b91fc1..773d06f 100644 --- a/hlpr_face_detection/launch/face_detector.launch +++ b/hlpr_face_detection/launch/face_detector.launch @@ -5,6 +5,6 @@ - + diff --git a/hlpr_face_detection/package.xml b/hlpr_face_detection/package.xml index c3929de..6b67a23 100644 --- a/hlpr_face_detection/package.xml +++ b/hlpr_face_detection/package.xml @@ -1,6 +1,6 @@ - face_detection + hlpr_face_detection 1.0.0 The face_detection package diff --git a/hlpr_face_detection/setup.py b/hlpr_face_detection/setup.py index 4d6cfda..0607335 100644 --- a/hlpr_face_detection/setup.py +++ b/hlpr_face_detection/setup.py @@ -5,7 +5,7 @@ # fetch values from package.xml setup_args = generate_distutils_setup( - packages=['face_detection'], + packages=['hlpr_face_detection'], package_dir={'': 'src'}, ) diff --git a/hlpr_face_detection/src/face_detector.py b/hlpr_face_detection/src/face_detector.py index 9b41ef0..ded14d2 100755 --- a/hlpr_face_detection/src/face_detector.py +++ b/hlpr_face_detection/src/face_detector.py @@ -5,7 +5,7 @@ from std_msgs.msg import String, Float32MultiArray, Bool from cv_bridge import CvBridge, CvBridgeError from sensor_msgs.msg import Image, PointCloud2, CompressedImage -from face_detection.msg import FaceDetectionTopic +from hlpr_face_detection.msg import FaceDetectionTopic import face_recognition class FaceDetector: From 6e584306707b31a3340e566df5567edb25e613c6 Mon Sep 17 00:00:00 2001 From: Akanksha Saran Date: Sat, 5 May 2018 00:26:21 -0500 Subject: [PATCH 21/28] Faster and more accurate CNN model to be used with GPU support --- hlpr_face_detection/launch/face_detector.launch | 2 ++ hlpr_face_detection/src/face_detector.py | 8 ++++++-- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/hlpr_face_detection/launch/face_detector.launch b/hlpr_face_detection/launch/face_detector.launch index 773d06f..f08e2a0 100644 --- a/hlpr_face_detection/launch/face_detector.launch +++ b/hlpr_face_detection/launch/face_detector.launch @@ -2,8 +2,10 @@ + + diff --git a/hlpr_face_detection/src/face_detector.py b/hlpr_face_detection/src/face_detector.py index ded14d2..17d0b9b 100755 --- a/hlpr_face_detection/src/face_detector.py +++ b/hlpr_face_detection/src/face_detector.py @@ -17,7 +17,7 @@ def __init__(self, tgtdir='.'): self.rgb_image = None self.tgtdir = tgtdir image_topic = rospy.get_param("/image_topic_name") - detection_sub = rospy.Subscriber(image_topic,Image, self.callback, queue_size=1, buff_size=52428800) + detection_sub = rospy.Subscriber(image_topic, Image, self.callback, queue_size=1, buff_size=52428800) rospy.sleep(5) print "Finished Init for Face Detector" @@ -36,7 +36,11 @@ def callback(self,rgb): def detect_faces(self, img): - faces = face_recognition.face_locations(img, number_of_times_to_upsample=1) + use_gpu = rospy.get_param("/use_gpu") + if(use_gpu): + faces = face_recognition.face_locations(img, number_of_times_to_upsample=1, model="cnn") + else: + faces = face_recognition.face_locations(img, number_of_times_to_upsample=1) multi_array = [] for face in faces: face_array = Float32MultiArray() From 5be046e45dd0cd3b520e523992f13d8ac7178afc Mon Sep 17 00:00:00 2001 From: Elaine Date: Mon, 7 May 2018 13:48:32 -0500 Subject: [PATCH 22/28] changed the way image topic is called so regular ros remapping will work --- hlpr_face_detection/src/face_detector.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/hlpr_face_detection/src/face_detector.py b/hlpr_face_detection/src/face_detector.py index 17d0b9b..39f8125 100755 --- a/hlpr_face_detection/src/face_detector.py +++ b/hlpr_face_detection/src/face_detector.py @@ -16,8 +16,7 @@ def __init__(self, tgtdir='.'): self.bridge = CvBridge() self.rgb_image = None self.tgtdir = tgtdir - image_topic = rospy.get_param("/image_topic_name") - detection_sub = rospy.Subscriber(image_topic, Image, self.callback, queue_size=1, buff_size=52428800) + detection_sub = rospy.Subscriber("image_topic", Image, self.callback, queue_size=1, buff_size=52428800) rospy.sleep(5) print "Finished Init for Face Detector" From 735d5663ea855cf1b337acc4893266c471f58dbc Mon Sep 17 00:00:00 2001 From: Akanksha Saran Date: Fri, 11 May 2018 12:58:26 -0500 Subject: [PATCH 23/28] Option to use the GPU version --- hlpr_face_detection/README.md | 10 +++++++++- hlpr_face_detection/launch/face_detector_gpu.launch | 12 ++++++++++++ 2 files changed, 21 insertions(+), 1 deletion(-) create mode 100644 hlpr_face_detection/launch/face_detector_gpu.launch diff --git a/hlpr_face_detection/README.md b/hlpr_face_detection/README.md index 9357359..9b20889 100644 --- a/hlpr_face_detection/README.md +++ b/hlpr_face_detection/README.md @@ -25,4 +25,12 @@ sudo pip install face_recognition sudo pip install git+https://github.com/ageitgey/face_recognition_models ```` -Check for more details. +Check for more details on the algorithm. + +# Test the face recognition package +```` +Without GPU: roslaunch hlpr_face_detection face_detector.launch +With GPU (more accurate version): roslaunch hlpr_face_detection face_detector_gpu.launch +```` + + diff --git a/hlpr_face_detection/launch/face_detector_gpu.launch b/hlpr_face_detection/launch/face_detector_gpu.launch new file mode 100644 index 0000000..4bd706f --- /dev/null +++ b/hlpr_face_detection/launch/face_detector_gpu.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + From 9256bfa865d294801ee7f67f11bf47c0b5258a9a Mon Sep 17 00:00:00 2001 From: mlchang Date: Tue, 19 Jun 2018 14:13:43 -0500 Subject: [PATCH 24/28] fixing bug where CMakeLists referenced indigo instead of catkin --- hlpr_single_plane_segmentation/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hlpr_single_plane_segmentation/CMakeLists.txt b/hlpr_single_plane_segmentation/CMakeLists.txt index f3ed801..43fdfcc 100644 --- a/hlpr_single_plane_segmentation/CMakeLists.txt +++ b/hlpr_single_plane_segmentation/CMakeLists.txt @@ -6,7 +6,7 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native") set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,--as-needed -pthread") set(OPENNI_INCLUDE_DIRS /usr/include/ni) -set(ROS_DISTRO indigo) +set(ROS_DISTRO kinetic) set(ROS_LIBRARIES /opt/ros/${ROS_DISTRO}/lib/libroscpp.so /opt/ros/${ROS_DISTRO}/lib/libroscpp_serialization.so /opt/ros/${ROS_DISTRO}/lib/librosconsole.so From 9f1d7f0aa2e5233cb977b1e9b1384fad5447e3cc Mon Sep 17 00:00:00 2001 From: Akanksha Saran Date: Fri, 29 Jun 2018 22:39:48 -0700 Subject: [PATCH 25/28] correct terminology --- hlpr_face_detection/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/hlpr_face_detection/README.md b/hlpr_face_detection/README.md index 9b20889..e6fff8c 100644 --- a/hlpr_face_detection/README.md +++ b/hlpr_face_detection/README.md @@ -18,7 +18,7 @@ $ cd .. $ sudo python setup.py install --no USE_AVX_INSTRUCTIONS ```` -# Install the face recognition package +# Install face_recognition ```` sudo pip install --upgrade scipy sudo pip install face_recognition @@ -27,7 +27,7 @@ sudo pip install git+https://github.com/ageitgey/face_recognition_models Check for more details on the algorithm. -# Test the face recognition package +# Test the face detection package ```` Without GPU: roslaunch hlpr_face_detection face_detector.launch With GPU (more accurate version): roslaunch hlpr_face_detection face_detector_gpu.launch From 50fa7f2010a0e61f1c4885c9d07f74f1c3a73ec9 Mon Sep 17 00:00:00 2001 From: ragtz Date: Sat, 1 Sep 2018 23:24:25 -0500 Subject: [PATCH 26/28] Delete cout as per review in pull request --- hlpr_feature_extraction/include/objectFeatures.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/hlpr_feature_extraction/include/objectFeatures.hpp b/hlpr_feature_extraction/include/objectFeatures.hpp index 75a342f..bf85fa7 100644 --- a/hlpr_feature_extraction/include/objectFeatures.hpp +++ b/hlpr_feature_extraction/include/objectFeatures.hpp @@ -73,8 +73,6 @@ class pc_cluster_features oriented_bounding_box.center.y = position(1); oriented_bounding_box.center.z = position(2); - std::cout<<"Center of OBB: ("<< position(0) <<", "< quat (rotational_matrix_OBB); oriented_bounding_box.rot_quat[0] = quat.x(); oriented_bounding_box.rot_quat[1] = quat.y(); From 222df84c1ea605dfa54f09769036c5a4a73012eb Mon Sep 17 00:00:00 2001 From: ragtz Date: Sat, 1 Sep 2018 23:27:49 -0500 Subject: [PATCH 27/28] Delete ColorHist as per comment in pull request --- .../include/objectFeatures.hpp | 99 ------------------- 1 file changed, 99 deletions(-) diff --git a/hlpr_feature_extraction/include/objectFeatures.hpp b/hlpr_feature_extraction/include/objectFeatures.hpp index bf85fa7..d432250 100644 --- a/hlpr_feature_extraction/include/objectFeatures.hpp +++ b/hlpr_feature_extraction/include/objectFeatures.hpp @@ -558,104 +558,5 @@ class HSHistogram{ } }; -// Helper class to help with colour histograms -/*class ColorHistogram { - private: - //std::vector > > hist3; - int dim; - public: - /*ColorHistogram(int dim):dim(dim) { - cloud_size = 0; - hist3.resize(dim); - for (int i = 0; i < dim; i++) { - hist3[i].resize(dim); - for (int j = 0; j < dim; j++) { - hist3[i][j].resize(dim); - std::fill( hist3[i][j].begin(), hist3[i][j].end(), 0 ); - } - } - }*/ - - //ColorHistogram(int dim):dim(dim){ } // empty constructor - - /*void computeHistogram(PointCloudT &cloud, std::vector > > &hist3, std::vector &hist3_double_vector) { - //ROS_INFO("Computing color histogram..."); - int cloud_size = cloud.points.size(); - for (int i = 0; i < cloud_size; i++) { - // Max value of 255. We want 256 because we want the rgb division to result in - // a value always slightly smaller than the dim due to index starting from 0 - double round = (double)256 / dim; - int r = (int)((double)(cloud.points[i].r) / round); - int g = (int)((double)(cloud.points[i].g) / round); - int b = (int)((double)(cloud.points[i].b) / round); - - //std::cout<<"\nValue before updating: "< toDoubleVector() { - int i_offset = dim * dim; - int j_offset = dim; - std::vector hist3_double_vector (dim * dim * dim, 0); - for (int i = 0; i < dim; i++) { - for (int j = 0; j < dim; j++) { - for (int k = 0; k < dim; k++) { - hist3_double_vector[i * i_offset + j * j_offset + k] = hist3[i][j][k]; - std::cout <<"\nValue of hist3: "< toDoubleVectorNormalized() { - std::vector hist_double_vector = toDoubleVector(); - for (int i = 0; i < hist_double_vector.size(); i++) { - hist_double_vector[i] /= cloud_size; - } - return hist_double_vector; - }*/ -//}; #endif /* OBJECTFEATURES_HPP_ */ From 5fba10639b1560d876a7d742defca4fa73a7254f Mon Sep 17 00:00:00 2001 From: ragtz Date: Sat, 1 Sep 2018 23:29:23 -0500 Subject: [PATCH 28/28] Delete commented code as per comment in pull request --- hlpr_feature_extraction/include/utils_pcl_ros.hpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/hlpr_feature_extraction/include/utils_pcl_ros.hpp b/hlpr_feature_extraction/include/utils_pcl_ros.hpp index 100cccb..5cd7c17 100644 --- a/hlpr_feature_extraction/include/utils_pcl_ros.hpp +++ b/hlpr_feature_extraction/include/utils_pcl_ros.hpp @@ -44,12 +44,8 @@ void Cube_2_Arrows(pcl::ModelCoefficients &cube, boost::shared_ptr &viewer, int index); Box3D -//boundingBoxWithCoeff(pcl::PointCloud &cluster, pcl::ModelCoefficients::Ptr coefficients); boundingBoxWithCoeff(pcl::PointCloud &cluster, pcl::ModelCoefficients::Ptr coefficients, pcl::PointCloud::Ptr &cloud_transformed); -//void -//fillRosMessage (hlpr_feature_extraction::PcFeatures &outRosMsg, const pc_cluster_features &inObjFeatures); - void fillBasicFeaturesMsg (hlpr_perception_msgs::BasicFeatures &basicInfo, const pc_cluster_features &inObjFeatures);