Пример #1
0
inline double
pcl::getAngle3D (const Eigen::Vector4f &v1, const Eigen::Vector4f &v2)
{
  // Compute the actual angle
  double rad = v1.dot (v2) / sqrt (v1.squaredNorm () * v2.squaredNorm ());
  if (rad < -1.0) rad = -1.0;
  if (rad >  1.0) rad = 1.0;
  return (acos (rad));
}
Пример #2
0
    void
    inputCallback (const sensor_msgs::PointCloud2ConstPtr &cloud_in)
    {
      if (output_cloud_.width != 0)
      {
        if (!object_released_)
        {
          output_cloud_.header.stamp = ros::Time::now();
          object_pub_.publish (output_cloud_);
          float *xzy = (float*)&output_cloud_.data[0];
          ROS_INFO("Republished object in frame %s (%g,%g,%g)", output_cloud_.header.frame_id.c_str(), xzy[0], xzy[1], xzy[2]);
        }
        return;
      }

      // If nothing is pointed at
      if (!to_select_)
        return;
      //      std::cerr << "to_select_" << std::endl;
      // If there is no TF data available
      ros::Time time = ros::Time::now();
      bool found_transform = tf_listener_.waitForTransform("right_hand", "openni_rgb_optical_frame", time, ros::Duration(1));
      if (!found_transform)
      {
        std::cerr << "no transform found" << std::endl;
        return;
      }
      tf::StampedTransform c2h_transform;
      tf_listener_.lookupTransform("right_hand", "openni_rgb_optical_frame", time, c2h_transform);
      to_select_ = false;

      ROS_INFO_STREAM ("[" << getName ().c_str () << "] Received cloud: cloud time " << cloud_in->header.stamp);

      // Filter the input dataser
      PointCloud cloud_raw, cloud;
      pcl::fromROSMsg (*cloud_in, cloud_raw);
      vgrid_.setInputCloud (boost::make_shared<PointCloud> (cloud_raw));
      vgrid_.filter (cloud);

      // Fit a plane (the table)
      pcl::ModelCoefficients table_coeff;
      pcl::PointIndices table_inliers;
      PointCloud cloud_projected;
      pcl::PointCloud<Point> cloud_hull;

      // ---[ Estimate the point normals
      pcl::PointCloud<pcl::Normal> cloud_normals;
      n3d_.setInputCloud (boost::make_shared<PointCloud> (cloud));
      n3d_.compute (cloud_normals);
      cloud_normals_.reset (new pcl::PointCloud<pcl::Normal> (cloud_normals));

      // TODO fabs? ... use parallel to Z.cross(X)!

      //z axis in Kinect frame
      btVector3 axis(0.0, 0.0, 1.0);
      //rotate axis around x in Kinect frame for an angle between base_link and head_tilt_link + 90deg
      //todo: get angle automatically
      btVector3 axis2 = axis.rotate(btVector3(1.0, 0.0, 0.0), btScalar(base_link_head_tilt_link_angle_ + pcl::deg2rad(90.0)));
      //std::cerr << "axis: " << fabs(axis2.getX()) << " " << fabs(axis2.getY()) << " " << fabs(axis2.getZ()) << std::endl;

      seg_.setInputCloud (boost::make_shared<PointCloud> (cloud));
      seg_.setInputNormals (cloud_normals_);
      seg_.setAxis (Eigen::Vector3f(fabs(axis2.getX()), fabs(axis2.getY()), fabs(axis2.getZ())));
      // seg_.setIndices (boost::make_shared<pcl::PointIndices> (selection));
      seg_.segment (table_inliers, table_coeff);
      ROS_INFO ("[%s] Table model: [%f, %f, %f, %f] with %d inliers.", getName ().c_str (),
                table_coeff.values[0], table_coeff.values[1], table_coeff.values[2], table_coeff.values[3], (int)table_inliers.indices.size ());
      if ((int)table_inliers.indices.size () <= min_table_inliers_)
      {
        ROS_ERROR ("table has to few inliers");
        return;
      }

      // Project the table inliers using the planar model coefficients
      proj_.setInputCloud (boost::make_shared<PointCloud> (cloud));
      proj_.setIndices (boost::make_shared<pcl::PointIndices> (table_inliers));
      proj_.setModelCoefficients (boost::make_shared<pcl::ModelCoefficients> (table_coeff));
      proj_.filter (cloud_projected);
      //cloud_pub_.publish (cloud_projected);

      // Create a Convex Hull representation of the projected inliers
      chull_.setInputCloud (boost::make_shared<PointCloud> (cloud_projected));
      chull_.reconstruct (cloud_hull);
      ROS_INFO ("Convex hull has: %d data points.", (int)cloud_hull.points.size ());
      hull_pub_.publish (cloud_hull);

      // //Compute the area of the plane
      // std::vector<double> plane_normal(3);
      // plane_normal[0] = table_coeff.values[0];
      // plane_normal[1] = table_coeff.values[1];
      // plane_normal[2] = table_coeff.values[2];
      // area_ = compute2DPolygonalArea (cloud_hull, plane_normal);
      // //ROS_INFO("[%s] Plane area: %f", getName ().c_str (), area_);
      // if (area_ > (expected_table_area_ + 0.01))
      // {
      //   extract_.setInputCloud (boost::make_shared<PointCloud> (cloud));
      //   extract_.setIndices (boost::make_shared<pcl::PointIndices> (table_inliers));
      //   extract_.setNegative (true);
      //   pcl::PointCloud<Point> cloud_extracted;
      //   extract_.filter (cloud_extracted);
      //   //cloud_extracted_pub_.publish (cloud_extracted);
      //   cloud = cloud_extracted;
      // }
      //end while
      //ROS_INFO ("[%s] Publishing convex hull with: %d data points and area %lf.", getName ().c_str (), (int)cloud_hull.points.size (), area_);
      //cloud_pub_.publish (cloud_hull);

      // pcl::PointXYZRGB point_min;
      // pcl::PointXYZRGB point_max;
      // pcl::PointXYZ point_center;
      // pcl::getMinMax3D (cloud_hull, point_min, point_max);
      // //Calculate the centroid of the hull
      // point_center.x = (point_max.x + point_min.x)/2;
      // point_center.y = (point_max.y + point_min.y)/2;
      // point_center.z = (point_max.z + point_min.z)/2;

      // tf::Transform transform;
      // transform.setOrigin( tf::Vector3(point_center.x, point_center.y, point_center.z));
      // transform.setRotation( tf::Quaternion(0, 0, 0) );
      // transform_broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(),
      //                                                           cloud_raw.header.frame_id, rot_table_frame_));

      // ---[ Get the objects on top of the table - from the raw cloud!
      pcl::PointIndices cloud_object_indices;
      prism_.setHeightLimits (cluster_min_height_, cluster_max_height_);
      prism_.setInputCloud (boost::make_shared<PointCloud> (cloud_raw));
      prism_.setInputPlanarHull (boost::make_shared<PointCloud>(cloud_hull));
      prism_.segment (cloud_object_indices);
      //ROS_INFO ("[%s] Number of object point indices: %d.", getName ().c_str (), (int)cloud_object_indices.indices.size ());

      pcl::PointCloud<Point> cloud_object;
      pcl::ExtractIndices<Point> extract_object_indices;
      //extract_object_indices.setInputCloud (cloud_all_minus_table_ptr);
      extract_object_indices.setInputCloud (boost::make_shared<PointCloud> (cloud_raw));
      extract_object_indices.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
      extract_object_indices.filter (cloud_object);
      //ROS_INFO ("[%s ] Publishing number of object point candidates: %d.", getName ().c_str (), (int)cloud_objects.points.size ());

      std::vector<pcl::PointIndices> clusters;
      cluster_.setInputCloud (boost::make_shared<PointCloud>(cloud_object));
      cluster_.setClusterTolerance (object_cluster_tolerance_);
      cluster_.setMinClusterSize (object_cluster_min_size_);
      cluster_.setMaxClusterSize (object_cluster_max_size_);
      cluster_.setSearchMethod (clusters_tree_);
      cluster_.extract (clusters);

      // TODO take closest to ray
      for (size_t i = 0; i < clusters.size (); i++)
      {
        // compute center and see if it is close enough to ray
        int cluster_center = clusters[i].indices[clusters[i].indices.size () / 2];
        Eigen::Vector4f pt = Eigen::Vector4f (cloud_object.points[cluster_center].x, cloud_object.points[cluster_center].y, cloud_object.points[cluster_center].z, 0);
        Eigen::Vector4f c = line_dir_.cross3 (line_point_ - pt); c[3] = 0;
#ifndef TEST
        if (c.squaredNorm () / line_dir_squaredNorm_ > 0.25*0.25) // further then 10cm
          continue;
#endif
        std::cerr << "found cluster" << std::endl;
        // TODO make more optimal? + ERRORS/INCONSISTENCY IN PCL: second formula and in c computation!

        //tf::transformPoint();
        //Eigen::Matrix4f transform_matrix;
        //pcl_ros::transformAsMatrix (c2h_transform, transform_matrix);

        // broadcast transform
//        tf::Transform fixed_transform;
//        fixed_transform.setOrigin (tf::Vector3(0.0, 0.0, 0.1));
//        fixed_transform.setRotation (tf::Quaternion(0, 0, 0));
//        transform_broadcaster_.sendTransform(tf::StampedTransform(fixed_transform, ros::Time::now(), "right_hand", "object_frame"));

        // transform object into right_hand frame
        pcl::PointCloud<Point> cloud_object_clustered ;
        pcl::copyPointCloud (cloud_object, clusters[i], cloud_object_clustered);
//        Eigen::Matrix4f eigen_transform;
//        pcl_ros::transformAsMatrix (c2h_transform, eigen_transform);
//        pcl::transformPointCloud(cloud_object_clustered, output_cloud_, eigen_transform);
//        output_cloud_.header.frame_id = ""right_hand";
        sensor_msgs::PointCloud2 cluster;
        pcl::toROSMsg (cloud_object_clustered, cluster);
//        template <typename PointT> void transformPointCloud (const pcl::PointCloud <PointT> &cloud_in, pcl::PointCloud <PointT> &cloud_out, const tf::Transform &transform);
//        void transformPointCloud (const std::string &target_frame, const tf::Transform &net_transform, const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2 &out);
        pcl_ros::transformPointCloud("right_hand", (tf::Transform)c2h_transform, cluster, output_cloud_);
        output_cloud_.header.frame_id = "right_hand"; // TODO needed?
//        for (unsigned cp = 0; cp < output_cloud_.points.size (); cp++)
//        {
//          output_cloud_.points[i].x -= output_cloud_.points[0].x;
//          output_cloud_.points[i].y -= output_cloud_.points[0].y;
//          output_cloud_.points[i].z -= output_cloud_.points[0].z;
//        }
//        cloud_objects_pub_.publish (output_cloud_);
        object_pub_.publish (output_cloud_);
        ROS_INFO("Published object with %d points", (int)clusters[i].indices.size ());

        // TODO get a nicer rectangle around the object
        unsigned center_idx = cloud_object_indices.indices.at (cluster_center);
        unsigned row = center_idx / 640;
        unsigned col = center_idx - row * 640;
        ros::ServiceClient client = nh_.serviceClient<kinect_cleanup::FilterObject>("/filter_object");
        kinect_cleanup::FilterObject srv;
        srv.request.min_row = std::max (0, (int)row-50);
        srv.request.min_col = std::max (0, (int)col-40);
        srv.request.max_row = std::min (479, (int)row+50);
        srv.request.max_col = std::min (479, (int)col+40);
        srv.request.rgb = cloud_raw.points[640 * srv.request.max_row + srv.request.max_col].rgb;
        for (int i=0; i<4; i++)
          srv.request.plane_normal[i] = table_coeff.values[i];
        if (client.call(srv))
          ROS_INFO("Object filter service responded: %s", srv.response.error.c_str ());
        else
          ROS_ERROR("Failed to call object filter service!");

        // take only 1 cluster into account
        break;
      }
    }
Пример #3
0
template <typename PointInT, typename PointNT, typename PointOutT> bool
pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computePairFeatures (
      const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
      int p_idx, int q_idx, float &f1, float &f2, float &f3, float &f4)
{
  // Compute the Cartesian difference between the two points
  Eigen::Vector4f delta = cloud.points[q_idx].getVector4fMap () - cloud.points[p_idx].getVector4fMap ();
  delta[3] = 0;

  // Compute the Euclidean norm = || p_idx - q_idx ||
  float distance_sqr = delta.squaredNorm ();

  if (distance_sqr == 0)
  {
    ROS_ERROR ("Euclidean distance between points %d and %d is 0!", p_idx, q_idx);
    f1 = f2 = f3 = f4 = 0;
    return (false);
  }

  // Estimate f4 = || delta ||
  f4 = sqrt (distance_sqr);

  // Create a Darboux frame coordinate system u-v-w
  // u = n1; v = (p_idx - q_idx) x u / || (p_idx - q_idx) x u ||; w = u x v
  pcl::Vector4fMapConst u = normals.points[p_idx].getNormalVector4fMap ();

  // Estimate f3 = u * delta / || delta ||
  // delta[3] = 0 (line 59)
  f3 = u.dot (delta) / f4;

  // v = delta * u
  Eigen::Vector4f v = Eigen::Vector4f::Zero ();
  v = delta.cross3 (u);

  distance_sqr = v.squaredNorm ();
  if (distance_sqr == 0)
  {
    ROS_ERROR ("Norm of Delta x U is 0 for point %d and %d!", p_idx, q_idx);
    f1 = f2 = f3 = f4 = 0;
    return (false);
  }

  // Copy the q_idx normal
  Eigen::Vector4f nq (normals.points[q_idx].normal_x,
                      normals.points[q_idx].normal_y,
                      normals.points[q_idx].normal_z,
                      0);

  // Normalize the vector
  v /= sqrt (distance_sqr);

  // Compute delta (w) = u x v
  delta = u.cross3 (v);

  // Compute f2 = v * n2;
  // v[3] = 0 (line 82)
  f2 = v.dot (nq);

  // Compute f1 = arctan (w * n2, u * n2) i.e. angle of n2 in the x=u, y=w coordinate system
  // delta[3] = 0 (line 59), nq[3] = 0 (line 97)
  f1 = atan2f (delta.dot (nq), u.dot (nq));       // @todo: optimize this

  return (true);
}