PointCloudRecognizer.h

Go to the documentation of this file.

#ifndef RAIL_PICK_AND_PLACE_POINT_CLOUD_RECOGNIZER_H_
#define RAIL_PICK_AND_PLACE_POINT_CLOUD_RECOGNIZER_H_

// RAIL Recognition
#include "PCLGraspModel.h"

// ROS
#include <graspdb/graspdb.h>
#include <sensor_msgs/PointCloud.h>
#include <ros/ros.h>
#include <rail_manipulation_msgs/SegmentedObject.h>
#include <tf2/LinearMath/Transform.h>

// PCL
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

namespace rail
{
namespace pick_and_place
{

class PointCloudRecognizer
{
public:
  static const double ALPHA = 0.5;
  static const double SCORE_CONFIDENCE_THRESHOLD = 1.1;
  static const double OVERLAP_THRESHOLD = 0.6;

  PointCloudRecognizer();

  bool recognizeObject(rail_manipulation_msgs::SegmentedObject &object,
      const std::vector<PCLGraspModel> &candidates) const;

private:
  double scoreRegistration(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr candidate,
      pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr object, tf2::Transform &tf_icp) const;

  void computeGraspList(const tf2::Transform &tf_icp, const geometry_msgs::Point &centroid,
      const std::vector<graspdb::Grasp> &candidate_grasps, std::vector<graspdb::Grasp> &grasps) const;
};

}
}

#endif