handle_detector.cpp

Go to the documentation of this file.

#include "handle_detector.h"

#include <pcl/ros/conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/common/transforms.h>

#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <Eigen/Geometry>
#include "visualize_polygon.hpp"
#include "polygon_filter.h"
#include <pcl/filters/passthrough.h>
#include <pcl/io/pcd_io.h>

#include <visualization_msgs/Marker.h>

#include <limits> //quiet_nan
#include <cmath> //atan2

DoorHandleDetector::DoorHandleDetector() : door_frame_("/door"), active_(false), 
                                           filter_polygon_filename_("filter_polygon.pcd"),
                                           door_radius_(1.05)
{
    ros::NodeHandle nh("~");
    nh.param<std::string>("door_frame", door_frame_, door_frame_);
    nh.param<bool>("start_active", active_, active_); //By default wait for activation by service call
    nh.param<double>("door_radius", door_radius_, door_radius_); //By default wait for activation by service call
    nh.param<std::string>("filter_polygon_filename", filter_polygon_filename_, filter_polygon_filename_);
    ROS_INFO("Using door frame: %s", door_frame_.c_str());

    if(active_) cloud_sub_.subscribe(node_, "cloud_in", 10);

    tf_filter_ = new tf::MessageFilter<PointCloudType>(cloud_sub_, tfListener_, door_frame_, 3);
    tf_filter_->registerCallback( boost::bind(&DoorHandleDetector::cloudCallback, this, _1) );

    door_cloud_publisher_ = node_.advertise<PointCloudType> ("door_cloud", 10, false);
    handle_cloud_publisher_ = node_.advertise<PointCloudType> ("handle_cloud", 10, false);
    door_state_publisher_  = node_.advertise<door_perception_msgs::DoorState> ("door_state", 10, false);
    door_state_service_ = node_.advertiseService("detect_door_state_in_cloud", &DoorHandleDetector::serviceCallback, this);
    vis_pub_ = node_.advertise<visualization_msgs::Marker>( "door_state_visualization", 2 );
    handle_pose_pub_ = node_.advertise<geometry_msgs::PoseStamped>( "door_handle", 3 );

    service_on_ = node_.advertiseService("handle_detection_on", &DoorHandleDetector::switchActive, this);
    service_off_ = node_.advertiseService("handle_detection_off", &DoorHandleDetector::switchInactive, this);

    if (pcl::io::loadPCDFile<PointType> (filter_polygon_filename_, cropping_polygon_) == -1){
      cropping_polygon_ = standardCroppingPolygon<PointType>(1.05f); //assumed door radius
      pcl::io::savePCDFileASCII (filter_polygon_filename_, cropping_polygon_);
    }

}



void DoorHandleDetector::cloudCallback(const PointCloudType::ConstPtr& msg){
  //TRANSFORMATION STUFF
    tf::StampedTransform to_door_frame;
    try {
      tfListener_.lookupTransform(door_frame_, msg->header.frame_id, msg->header.stamp, to_door_frame);
    }
    catch (tf::TransformException ex){
      ROS_WARN("%s",ex.what());
    }

    Eigen::Matrix4f eigen_transform;
    pcl_ros::transformAsMatrix(to_door_frame, eigen_transform);
    //tf::Transform sensor_in_door_frame = to_door_frame.inverse();
    
    PointCloudType transformed_cloud;
    pcl::transformPointCloud(*msg,transformed_cloud,eigen_transform);
    transformed_cloud.header.frame_id = door_frame_;

    PointCloudType inside_cloud = crop_by_polygon(cropping_polygon_, transformed_cloud, 0, 1);
    inside_cloud.header = transformed_cloud.header;
  //PROCESSING DOOR CLOUD
    if(inside_cloud.size() == 0){
      ROS_INFO("No points found in door region");
      return;
    }
    else
    {
      // Create the passthrough filtering object
      PointCloudType::Ptr cloud_filtered(new PointCloudType);
      pcl::PassThrough<PointType> pass;
      pass.setInputCloud (inside_cloud.makeShared());
      pass.setFilterFieldName ("z");
      pass.setFilterLimits (-0.5, 0.5); //Assumption: Handle is within half a meter of the door (tf) frame height
      pass.filter (*cloud_filtered);
      cloud_filtered->header = inside_cloud.header;

      PointCloudType handle;
      handle.header = inside_cloud.header;
      estimateDoorHandle(*cloud_filtered, handle);
      //Visualize
      //send handle cloud
      if(handle_cloud_publisher_.getNumSubscribers() > 0){
        handle_cloud_publisher_.publish(handle);
      }
    }

  //VISUALIZATION
    if(vis_pub_.getNumSubscribers() > 0){
      cropping_polygon_.header.frame_id = door_frame_;
      cropping_polygon_.header.stamp = msg->header.stamp;//Important for visualization
      visualization_msgs::Marker polygon = visualize_polygon(cropping_polygon_);
      vis_pub_.publish(polygon);
    }
}


bool DoorHandleDetector::doorHandleRansac(const PointCloudType& cloud, PointCloudType& handle_cloud, Eigen::Vector3f& door_normal, double& plane_distance_to_origin )
{
  if(cloud.size() == 0){
    ROS_INFO("Cannot estimate door from empty cloud. Open?");
    return false;
  }
  ROS_WARN_COND(cloud.header.frame_id != (door_frame_), "Handle detection assumes cloud to be in the door frame (%s), not in %s", door_frame_.c_str(), cloud.header.frame_id.c_str());

  pcl::ModelCoefficients door_coeff;
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  seg.setOptimizeCoefficients (true); // Optional
  seg.setModelType (pcl::SACMODEL_PLANE); // Mandatory
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold (0.025);

  seg.setInputCloud (cloud.makeShared ());
  seg.segment (*inliers, door_coeff);

  if (inliers->indices.size () == 0)
  {
    ROS_WARN ("Could not estimate a planar model for the given cloud.");
    return false;
  }

  door_normal << door_coeff.values[0], door_coeff.values[1], door_coeff.values[2];
  door_normal.normalize();
  ROS_DEBUG("Normal xyz=(%.2f %.2f %.2f)", door_normal(0), door_normal(1), door_normal(2));
  ROS_WARN_COND(abs(door_normal(2)) > 0.1, "Door normal Z should be minimal!");
  plane_distance_to_origin = door_coeff.values[3];
  
  if (cloud.size() - inliers->indices.size () < 10) //minimum number of points to consider a handle
  {
    ROS_INFO_THROTTLE (10, "Found door, but there are too few points left for handle. (throttled to 1msg/10s)");
    return false;
  }
  // Extract the door plane outliers
  PointCloudType potential_handle;
  pcl::ExtractIndices<PointType> outlier_extractor;
  outlier_extractor.setInputCloud(cloud.makeShared());
  outlier_extractor.setIndices(inliers);
  outlier_extractor.setNegative(true);
  outlier_extractor.filter(potential_handle);
  ROS_DEBUG_STREAM("Door Outliers (Potential Handle): " << potential_handle.size());

  if(door_cloud_publisher_.getNumSubscribers() > 0){
    // Extract the door plane inliers
    PointCloudType door_cloud;
    outlier_extractor.setNegative(false);
    outlier_extractor.filter(door_cloud);
    ROS_DEBUG_STREAM("Door Inliers: " << door_cloud.size());
    door_cloud_publisher_.publish(door_cloud);
  }

  //Fit Line
  pcl::ModelCoefficients handle_coeff;
  pcl::PointIndices::Ptr h_inliers (new pcl::PointIndices);
  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZ> seg2;
  seg2.setOptimizeCoefficients (true); // Optional
  seg2.setModelType (pcl::SACMODEL_LINE); // Mandatory
  seg2.setMethodType (pcl::SAC_RANSAC);
  seg2.setDistanceThreshold (0.02);

  seg2.setInputCloud (potential_handle.makeShared ());
  seg2.segment (*h_inliers, handle_coeff);

  if (h_inliers->indices.size () < 10)
  {
    ROS_WARN ("Could not estimate a line model for the given handle.");
    return false;
  }
  ROS_WARN_COND(abs(handle_coeff.values[2]) > 0.1, "Handle axis Z should be minimal!");
  if(abs(handle_coeff.values[2]) > 0.3){
      ROS_WARN("Handle proposal rejected: Z (up/down) direction of handle should be minimal! Handle axis: (%.3f %.3f %.3f)", handle_coeff.values[0], handle_coeff.values[1], handle_coeff.values[2]);
      return false;
  }
  // Extract the handle line inliers
  
  pcl::ExtractIndices<PointType> inlier_extractor;
  inlier_extractor.setInputCloud(potential_handle.makeShared());
  inlier_extractor.setIndices(h_inliers);
  inlier_extractor.setNegative(false);
  inlier_extractor.filter(handle_cloud);
  ROS_DEBUG_STREAM("Handle Inliers (Handle): " << handle_cloud.size());
  return true;
}


//TODO: Ground Filter!
//TODO: If performance becomes a problem, 1st project cropping polygon to
//sensor frame, then crop, fit plane and send handle out in sensor frame.
//Computes the door plane and the (smaller) angle to the Y axis
void DoorHandleDetector::estimateDoorHandle(const PointCloudType& cloud, PointCloudType& handle_cloud)
{
  Eigen::Vector3f door_normal(0,0,0);
  double  plane_dist_from_origin;
  if(!doorHandleRansac(cloud, handle_cloud, door_normal, plane_dist_from_origin)){
    door_state_msg_.header = cloud.header;; 
    door_state_msg_.door_found = false;
    door_state_msg_.angle = 0.0;
    if(door_state_publisher_.getNumSubscribers() > 0){
      door_state_publisher_.publish(door_state_msg_);
    }
    return; //No Valid Handle
  }

  //Door State (for service and publishing
  Eigen::Vector3f door_line(-door_normal(1),door_normal(0),0); //perpendicular to door normal
  double angle = atan2(door_line(1), door_line(0));//y and x equivalent, see below
  //Clamp to -20° to 160°
  if(angle < -20.0/180.0*3.1415927){
    angle += 3.1415927;
  }
  if(angle > 160.0/180.0*3.1415927){
    angle -= 3.1415927;
  }

  door_state_msg_.header = cloud.header;; 
  door_state_msg_.door_found = true;
  door_state_msg_.angle = angle;
  if(door_state_publisher_.getNumSubscribers() > 0){
    door_state_publisher_.publish(door_state_msg_);
  }

  //Computation of the Desired Gripper Transformations #########################
  Eigen::Vector3f handle_mean, handle_axis;
  if(!computeMainAxis(handle_cloud, handle_mean, handle_axis, 15.0)) return; //main axis eval is 100 times next eval
  //std::cout << "\nReturned handle_axis:\n" << handle_axis << "\nReturned handle_Mean:\n" << handle_mean << std::endl;
  if (!((handle_axis.array() == handle_axis.array())).all() || //checking for nan
      !((handle_mean.array() == handle_mean.array())).all()) 
  {
    ROS_WARN("No valid axis for grasp");
    return;
  }
  //ROS_INFO_THROTTLE(5,"Mean xyz=(%.2f %.2f %.2f)", handle_mean(0), handle_mean(1), handle_mean(2));

//  http://mathworld.wolfram.com/HessianNormalForm.html
  double handle_dist_from_door = door_normal.dot(handle_mean) + plane_dist_from_origin;
  // X axis shall point into the door, so 
  if(handle_dist_from_door > 0){ //if door normal points towards handle,... 
    door_normal *= -1.0;//flip it
  }
  // Y Axis towards door hinge
  if(handle_mean.dot(handle_axis) > 0){ //also points in the direction hinge->handle
    handle_axis *= -1.0; //should point from handle->hinge
  }
  Eigen::Vector3f door_axis = handle_axis.cross(door_normal); // z-axis cross x-axis = y axis
  Eigen::Matrix3f grm;//gripper_rot_mat;
  grm << door_normal, door_axis, handle_axis;
  //ROS_INFO_STREAM_THROTTLE(5, "Desired Gripper Coord Frame\n: " << grm);
  //This is the desired gripper coord frame for the handle grasp
  tf::Matrix3x3 toGripperTargetCoordFrameMat(grm(0,0), grm(0,1), grm(0,2), 
                                             grm(1,0), grm(1,1), grm(1,2), 
                                             grm(2,0), grm(2,1), grm(2,2)) ;
  tf::Vector3 toGripperTargetCoordFrameVec(handle_mean(0), handle_mean(1), handle_mean(2));
  tf::Transform handlePose(toGripperTargetCoordFrameMat, toGripperTargetCoordFrameVec);
  tf::StampedTransform handlePoseStamped(handlePose, handle_cloud.header.stamp, handle_cloud.header.frame_id, handle_cloud.header.frame_id+"_handle");
  br_.sendTransform(handlePoseStamped);
  
  //Conversion madness
  tf::Stamped<tf::Pose> handlePoseStamped2(handlePoseStamped, handlePoseStamped.stamp_, handlePoseStamped.frame_id_);
  geometry_msgs::PoseStamped handlePoseMsg;
  tf::poseStampedTFToMsg(handlePoseStamped2, handlePoseMsg);

  if (handle_pose_pub_.getNumSubscribers() > 0){
    handle_pose_pub_.publish(handlePoseMsg);
  }
 
}

bool DoorHandleDetector::switchActive(std_srvs::Empty::Request  &req,
                                      std_srvs::Empty::Response &res )
{
  active_ = true;
  cloud_sub_.subscribe(node_, "cloud_in", 10);
  ROS_INFO("Handle detection (from pointcloud) activated");
  return true;
}
bool DoorHandleDetector::switchInactive(std_srvs::Empty::Request  &req,
                                        std_srvs::Empty::Response &res )
{
  active_ = false;
  cloud_sub_.unsubscribe();
  ROS_INFO("Handle detection (from pointcloud) deactivated");
  return true;
}

//Send out last result
bool DoorHandleDetector::serviceCallback(door_perception_msgs::DoorStateSrv::Request& request, door_perception_msgs::DoorStateSrv::Response& response){
    response.state =door_state_msg_;    
    return true;
}

bool computeMainAxis(const PointCloudType& pc, Eigen::Vector3f& mean, Eigen::Vector3f& main_axis, float eval_ratio_threshold)
{
  using namespace Eigen;

  mean = Vector3f::Zero();
  for(unsigned int i = 0; i < pc.size(); i++){
    const PointType& p = pc.points.at(i);
    mean += p.getVector3fMap ();
  }
  mean /= static_cast<float>(pc.size());

  Matrix3f cov = Matrix3f::Zero();
  for(unsigned int i = 0; i < pc.size(); i++){
    const PointType& p = pc.points.at(i);
    Vector3f v = p.getVector3fMap() - mean;
    cov += v * v.adjoint();
  }
  cov /= static_cast<float>(pc.size());

  JacobiSVD<Matrix3f> svd(cov, ComputeFullU);
  Matrix3f eigenvectors = svd.matrixU();
  //use first col as main eigenvector
  main_axis = eigenvectors.col(0); 

  //std::cout << "EigenVectors:" << std::endl << eigenvectors << std::endl;
  //std::cout << "EigenValues:\n" << svd.singularValues() << std::endl;
  Vector3f evals = svd.singularValues();
  if(evals(0) / evals(1) < eval_ratio_threshold){
      ROS_WARN_THROTTLE(5, "Low Eigenvalue Ratio: %f", (evals(0) / evals(1)));
      return false;
  }else 
      return true;
}