PointCloudRecognizer.cpp
Go to the documentation of this file.
00001 
00013 // RAIL Recognition
00014 #include "rail_recognition/PointCloudMetrics.h"
00015 #include "rail_recognition/PointCloudRecognizer.h"
00016 
00017 // PCL
00018 #include <pcl/kdtree/kdtree_flann.h>
00019 #include <pcl/registration/icp.h>
00020 
00021 using namespace std;
00022 using namespace rail::pick_and_place;
00023 
00024 PointCloudRecognizer::PointCloudRecognizer()
00025 {
00026 }
00027 
00028 bool PointCloudRecognizer::recognizeObject(rail_manipulation_msgs::SegmentedObject &object,
00029     const vector<PCLGraspModel> &candidates) const
00030 {
00031   // make sure we have some candidates
00032   if (candidates.empty())
00033   {
00034     ROS_WARN("Candidate object list is empty. Nothing to compare segmented object to.");
00035     return false;
00036   }
00037   if (object.point_cloud.data.empty())
00038   {
00039     ROS_WARN("Segmented object point cloud is empty. Nothing to compare candidate objects to.");
00040     return false;
00041   }
00042 
00043   // convert to a PCL point cloud
00044   pcl::PointCloud<pcl::PointXYZRGB>::Ptr object_point_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
00045   point_cloud_metrics::rosPointCloud2ToPCLPointCloud(object.point_cloud, object_point_cloud);
00046 
00047   // pre-process input cloud
00048   double object_r, object_g, object_b, object_std_dev_r, object_std_dev_g, object_std_dev_b;
00049   point_cloud_metrics::calculateAvgColors(object_point_cloud, object_r, object_g, object_b);
00050   point_cloud_metrics::calculateStdDevColors(object_point_cloud, object_std_dev_r, object_std_dev_g,
00051                                              object_std_dev_b, object_r, object_g, object_b);
00052   point_cloud_metrics::filterPointCloudOutliers(object_point_cloud);
00053   point_cloud_metrics::transformToOrigin(object_point_cloud, object.centroid);
00054 
00055   // perform recognition
00056   double min_score = numeric_limits<double>::infinity();
00057   size_t min_index;
00058   tf2::Transform min_icp_tf;
00059   for (size_t i = 0; i < candidates.size(); i++)
00060   {
00061     const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &candidate_point_cloud = candidates[i].getPCLPointCloud();
00062 
00063     // quick check for a valid point cloud
00064     if (!candidate_point_cloud->empty())
00065     {
00066       // do an average color check
00067 //      if (fabs(object_r - candidates[i].getAverageRed()) <= object_std_dev_r / 0.75
00068 //          && fabs(object_g - candidates[i].getAverageGreen()) <= object_std_dev_g / 0.75
00069 //          && fabs(object_b - candidates[i].getAverageBlue()) <= object_std_dev_b / 0.75)
00070 //      {
00071         tf2::Transform cur_icp_tf;
00072         double score = this->scoreRegistration(candidate_point_cloud, object_point_cloud, cur_icp_tf);
00073         if (score < min_score)
00074         {
00075           min_score = score;
00076           min_index = i;
00077           min_icp_tf = cur_icp_tf;
00078         }
00079 //      }
00080 //      else
00081 //      {
00082 //        ROS_INFO("Failed color check for model %s", candidates[i].getObjectName().c_str());
00083 //      }
00084     }
00085   }
00086 
00087   // check if there is enough confidence
00088   if (min_score > SCORE_CONFIDENCE_THRESHOLD)
00089   {
00090     ROS_INFO("Minimum recognition score did not exceed confidence threshold.");
00091     ROS_INFO("Min score: %lf", min_score);
00092     return false;
00093   }
00094     ROS_INFO("Object successfully recognized as: %s", object.name.c_str());
00095 
00096   // fill in recognition information
00097   object.name = candidates[min_index].getObjectName();
00098   object.model_id = candidates[min_index].getID();
00099   object.confidence = min_score;
00100   object.recognized = true;
00101   object.orientation.w = 1.0;
00102   object.grasps.clear();
00103 
00104   // extract possible grasps for this model
00105   vector<graspdb::Grasp> possible_grasps;
00106   this->computeGraspList(min_icp_tf, object.centroid, candidates[min_index].getGrasps(), possible_grasps);
00107 
00108   // sort and remove any grasps with 0 success rates
00109   vector<double> success_rates;
00110   for (size_t i = 0; i < possible_grasps.size(); i++)
00111   {
00112     double rate = possible_grasps[i].getSuccessRate();
00113     geometry_msgs::PoseStamped pose = possible_grasps[i].getGraspPose().toROSPoseStampedMessage();
00114     // fix the frame ID
00115     pose.header.frame_id = object.point_cloud.header.frame_id;
00116 
00117     // check the success rate -- any non-zero or non-attempted grasp
00118     if (rate > 0 || possible_grasps[i].getAttempts() == 0)
00119     {
00120       // place it in order
00121       bool inserted = false;
00122       rail_manipulation_msgs::Grasp grasp;
00123       grasp.grasp_pose = pose;
00124       grasp.attempts = possible_grasps[i].getAttempts();
00125       grasp.successes = possible_grasps[i].getSuccesses();
00126       grasp.eef_frame_id = possible_grasps[i].getEefFrameID();
00127       for (size_t j = 0; j < success_rates.size(); j++)
00128       {
00129         if (rate <= success_rates[j])
00130         {
00131           object.grasps.insert(object.grasps.begin() + j, grasp);
00132           success_rates.insert(success_rates.begin() + j, rate);
00133           inserted = true;
00134           break;
00135         }
00136       }
00137 
00138       if (!inserted)
00139       {
00140         object.grasps.push_back(grasp);
00141         success_rates.push_back(rate);
00142       }
00143     }
00144   }
00145 
00146   return true;
00147 }
00148 
00149 double PointCloudRecognizer::scoreRegistration(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr candidate,
00150     pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr object, tf2::Transform &tf_icp) const
00151 {
00152   // use ICP to for matching
00153   pcl::PointCloud<pcl::PointXYZRGB>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZRGB>);
00154   tf_icp = point_cloud_metrics::performICP(candidate, object, aligned);
00155 
00156   // check overlap first to determine if a the registration should be scored further
00157   double overlap, color_error;
00158   point_cloud_metrics::calculateRegistrationMetricOverlap(candidate, aligned, overlap, color_error);
00159   if (overlap < OVERLAP_THRESHOLD)
00160   {
00161     return numeric_limits<double>::infinity();
00162   }
00163 
00164   // calculate the distance and color error
00165   double distance_error = point_cloud_metrics::calculateRegistrationMetricDistanceError(candidate, aligned);
00166 
00167   // calculate the final weighted result
00168   double result = ALPHA * (3.0 * distance_error) + (1.0 - ALPHA) * (color_error / 100.0);
00169   return result;
00170 }
00171 
00172 void PointCloudRecognizer::computeGraspList(const tf2::Transform &tf_icp, const geometry_msgs::Point &centroid,
00173     const vector<graspdb::Grasp> &candidate_grasps, vector<graspdb::Grasp> &grasps) const
00174 {
00175   // ensure an empty list
00176   grasps.clear();
00177 
00178   // transform each pose
00179   for (size_t i = 0; i < candidate_grasps.size(); i++)
00180   {
00181     // convert to tf2 matrix
00182     tf2::Transform tf_pose = candidate_grasps[i].getGraspPose().toTF2Transform();
00183 
00184     // push back the basic information
00185     grasps.push_back(candidate_grasps[i]);
00186 
00187     // use the inverse for the result
00188     tf2::Transform result = tf_icp.inverseTimes(tf_pose);
00189 
00190     // correct for the origin transform
00191     result.getOrigin().setX(result.getOrigin().getX() + centroid.x);
00192     result.getOrigin().setY(result.getOrigin().getY() + centroid.y);
00193     result.getOrigin().setZ(result.getOrigin().getZ() + centroid.z);
00194 
00195     // copy over the values
00196     grasps[i].setGraspPose(graspdb::Pose(grasps[i].getGraspPose().getRobotFixedFrameID(), result));
00197   }
00198 }


rail_recognition
Author(s): David Kent , Russell Toris , bhetherman
autogenerated on Thu Jun 6 2019 19:44:08