The main recognition object for segmented point clouds. More...
#include <PointCloudRecognizer.h>
Public Member Functions | |
PointCloudRecognizer () | |
bool | recognizeObject (rail_manipulation_msgs::SegmentedObject &object, const std::vector< PCLGraspModel > &candidates) const |
The main recognition function. | |
Static Public Attributes | |
static const double | ALPHA = 0.5 |
static const double | OVERLAP_THRESHOLD = 0.6 |
static const double | SCORE_CONFIDENCE_THRESHOLD = 1.1 |
Private Member Functions | |
void | computeGraspList (const tf2::Transform &tf_icp, const geometry_msgs::Point ¢roid, const std::vector< graspdb::Grasp > &candidate_grasps, std::vector< graspdb::Grasp > &grasps) const |
Compute the grasps for the recognized object. | |
double | scoreRegistration (pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr candidate, pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr object, tf2::Transform &tf_icp) const |
Score the point cloud registration for the two point clouds. |
The main recognition object for segmented point clouds.
The point cloud recognizer takes a segmented object and a list of grasp model candidates and attempts to recognize the object.
Definition at line 42 of file PointCloudRecognizer.h.
Creates a new PointCloudRecognizer.
Definition at line 24 of file PointCloudRecognizer.cpp.
void PointCloudRecognizer::computeGraspList | ( | const tf2::Transform & | tf_icp, |
const geometry_msgs::Point & | centroid, | ||
const std::vector< graspdb::Grasp > & | candidate_grasps, | ||
std::vector< graspdb::Grasp > & | grasps | ||
) | const [private] |
Compute the grasps for the recognized object.
Compute the grasps to the recognized object based on the grasps from the model.
tf_icp | The transform between the candidate model and the segmented object. |
centroid | The centroid of the object point cloud. |
candidate_grasps | The candidate grasps from the model. |
grasps | The transformed grasps with respect to the recognized object. |
Definition at line 172 of file PointCloudRecognizer.cpp.
bool PointCloudRecognizer::recognizeObject | ( | rail_manipulation_msgs::SegmentedObject & | object, |
const std::vector< PCLGraspModel > & | candidates | ||
) | const |
The main recognition function.
Attempt to recognize the given object against the list of candidates. The object is compared to each candidate and registration metrics are calculated. The weighted score is checked and the model with the lowest error score is picked as the object. If this score meets the threshold, the segmented object is updated with the correct grasps and object information.
object | The segmented object to recognize and update if recognition is successful. |
candidates | The list of candidate models for this object. |
Definition at line 28 of file PointCloudRecognizer.cpp.
double PointCloudRecognizer::scoreRegistration | ( | pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr | candidate, |
pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr | object, | ||
tf2::Transform & | tf_icp | ||
) | const [private] |
Score the point cloud registration for the two point clouds.
Perform registration from the object to the candidate and return the resulting weighted registration score (a measure of error). The tf_icp transform is filled with the transform used to shift the object to the candidate. A score of infinity (meaning a very poor match) is possible.
candidate | The candidate point cloud. |
object | The point cloud of the object in question. |
tf_icp | The transform from object to candidate used after ICP. |
Definition at line 149 of file PointCloudRecognizer.cpp.
const double rail::pick_and_place::PointCloudRecognizer::ALPHA = 0.5 [static] |
The weighting constant for the match score metric.
Definition at line 46 of file PointCloudRecognizer.h.
const double rail::pick_and_place::PointCloudRecognizer::OVERLAP_THRESHOLD = 0.6 [static] |
The threshold for the overlap metric to be considered a valid match.
Definition at line 50 of file PointCloudRecognizer.h.
const double rail::pick_and_place::PointCloudRecognizer::SCORE_CONFIDENCE_THRESHOLD = 1.1 [static] |
The confidence threshold for a recognition score.
Definition at line 48 of file PointCloudRecognizer.h.