00001 #ifndef shape_features_h_DEFINED
00002 #define shape_features_h_DEFINED
00003
00004 #include <ros/ros.h>
00005 #include <opencv2/core/core.hpp>
00006 #include <pcl16/point_cloud.h>
00007 #include <pcl16/point_types.h>
00008 #include <cpl_visual_features/features/shape_context.h>
00009 #include <vector>
00010 #include <tabletop_pushing/point_cloud_segmentation.h>
00011 #include <tabletop_pushing/VisFeedbackPushTrackingAction.h>
00012
00013 namespace tabletop_pushing
00014 {
00015
00016 class ShapeLocation
00017 {
00018 public:
00019 ShapeLocation(pcl16::PointXYZ boundary_loc, cpl_visual_features::ShapeDescriptor descriptor):
00020 boundary_loc_(boundary_loc), descriptor_(descriptor)
00021 {
00022 }
00023 pcl16::PointXYZ boundary_loc_;
00024 cpl_visual_features::ShapeDescriptor descriptor_;
00025
00026 ShapeLocation() : boundary_loc_(0.0,0.0,0.0), descriptor_()
00027 {
00028 }
00029 };
00030
00031 typedef std::vector<ShapeLocation> ShapeLocations;
00032
00033
00034
00035 cv::Mat getObjectFootprint(cv::Mat obj_mask, XYZPointCloud& cloud);
00036
00037 void getPointRangesXY(XYZPointCloud& samples, cpl_visual_features::ShapeDescriptor& sd);
00038
00039 void getCovarianceXYFromPoints(XYZPointCloud& pts, cpl_visual_features::ShapeDescriptor& sd);
00040
00041 void extractPCAFeaturesXY(XYZPointCloud& samples, cpl_visual_features::ShapeDescriptor& sd);
00042
00043 void extractBoundingBoxFeatures(XYZPointCloud& samples, cpl_visual_features::ShapeDescriptor& sd);
00044
00045 XYZPointCloud getObjectBoundarySamples(ProtoObject& cur_obj, double hull_alpha = 0.01);
00046
00047 cpl_visual_features::Path compareBoundaryShapes(XYZPointCloud& hull_a, XYZPointCloud& hull_b,
00048 double& min_cost, double epsilon_cost = 0.99);
00049
00050 void estimateTransformFromMatches(XYZPointCloud& cloud_t_0, XYZPointCloud& cloud_t_1,
00051 cpl_visual_features::Path p, Eigen::Matrix4f& transform, double max_dist=0.3);
00052
00053 cv::Mat visualizeObjectBoundarySamples(XYZPointCloud& hull_cloud,
00054 tabletop_pushing::VisFeedbackPushTrackingFeedback& cur_state);
00055
00056 cv::Mat visualizeObjectBoundaryMatches(XYZPointCloud& hull_a, XYZPointCloud& hull_b,
00057 tabletop_pushing::VisFeedbackPushTrackingFeedback& cur_state,
00058 cpl_visual_features::Path& path);
00059
00060 cv::Mat visualizeObjectContactLocation(XYZPointCloud& hull_cloud,
00061 tabletop_pushing::VisFeedbackPushTrackingFeedback& cur_state,
00062 pcl16::PointXYZ& contact_pt, pcl16::PointXYZ& forward_pt);
00063
00064 ShapeLocations extractObjectShapeContext(ProtoObject& cur_obj, bool use_center = true);
00065
00066 ShapeLocations extractShapeContextFromSamples(XYZPointCloud& samples_pcl,
00067 ProtoObject& cur_obj, bool use_center);
00068
00069 XYZPointCloud transformSamplesIntoSampleLocFrame(XYZPointCloud& samples, ProtoObject& cur_obj,
00070 pcl16::PointXYZ sample_pt);
00071
00072 cpl_visual_features::ShapeDescriptor extractPointHistogramXY(XYZPointCloud& samples, double x_res, double y_res, double x_range,
00073 double y_range);
00074
00075
00076 XYZPointCloud getLocalSamples(XYZPointCloud& samples_pcl, ProtoObject& cur_obj, pcl16::PointXYZ sample_loc,
00077 double s, double hull_alpha);
00078
00079 cpl_visual_features::ShapeDescriptors extractLocalAndGlobalShapeFeatures(XYZPointCloud& hull, ProtoObject& cur_obj,
00080 double sample_spread, double hull_alpha,
00081 double hist_res);
00082
00083 cpl_visual_features::ShapeDescriptor extractLocalAndGlobalShapeFeatures(XYZPointCloud& hull, ProtoObject& cur_obj,
00084 pcl16::PointXYZ sample_pt, int sample_pt_idx,
00085 double sample_spread, double hull_alpha,
00086 double hist_res);
00087
00088 cpl_visual_features::ShapeDescriptor extractLocalShapeFeatures(XYZPointCloud& samples_pcl,
00089 ProtoObject& cur_obj, pcl16::PointXYZ sample_loc,
00090 double s, double hull_alpha, double hist_res);
00091
00092 cv::Mat computeShapeFeatureAffinityMatrix(ShapeLocations& locs, bool use_center = false);
00093
00094 double shapeFeatureChiSquareDist(cpl_visual_features::ShapeDescriptor& a, cpl_visual_features::ShapeDescriptor& b, double gamma=0.0);
00095
00096 double shapeFeatureSquaredEuclideanDist(cpl_visual_features::ShapeDescriptor& a, cpl_visual_features::ShapeDescriptor& b);
00097
00098 void clusterShapeFeatures(ShapeLocations& locs, int k, std::vector<int>& cluster_ids,
00099 cpl_visual_features::ShapeDescriptors& centers, double min_err_change, int max_iter,
00100 int num_retries = 5);
00101
00102 int closestShapeFeatureCluster(cpl_visual_features::ShapeDescriptor& descriptor,
00103 cpl_visual_features::ShapeDescriptors& centers, double& min_dist);
00104
00105 cpl_visual_features::ShapeDescriptors loadSVRTrainingFeatures(std::string feature_path, int feat_length);
00106
00107 cv::Mat computeChi2Kernel(cpl_visual_features::ShapeDescriptors& sds, std::string feat_path, int local_length,
00108 int global_length, double gamma_local, double gamma_global, double mixture_weight);
00109
00110 };
00111 #endif // shape_features_h_DEFINED