Functions | |
void | calculateAvgColors (const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &pc, double &avg_r, double &avg_g, double &avg_b) |
Average color value calculator. | |
double | calculateRegistrationMetricDistanceError (const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &base, const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &target) |
Point cloud distance error metric calculator. | |
void | calculateRegistrationMetricOverlap (const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &base, const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &target, double &overlap, double &color_error, const double metric_overlap_search_radius=DEFAULT_METRIC_OVERLAP_SEARCH_RADIUS) |
Point cloud overlap metric calculator. | |
double | calculateStdDevColors (const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &pc, double &std_dev_r, double &std_dev_g, double &std_dev_b) |
Color value standard deviation calculator. | |
double | calculateStdDevColors (const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &pc, double &std_dev_r, double &std_dev_g, double &std_dev_b, const double avg_r, const double avg_g, const double avg_b) |
Color value standard deviation calculator. | |
bool | classifyMerge (const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &base, const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &target) |
Classify the point cloud merge. | |
geometry_msgs::Point | computeCentroid (const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &pc) |
Compute the centroid of the given point cloud. | |
void | filterPointCloudOutliers (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &pc, const double filter_outlier_search_radius=DEFAULT_FILTER_OUTLIER_SEARCH_RADIUS, const double filter_outlier_min_num_neighbors=DEFAULT_FILTER_OUTLIER_MIN_NUM_NEIGHBORS) |
Filter point cloud outliers. | |
void | filterRedundantPoints (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &pc, const double filter_redundant_search_radius=DEFAULT_FILTER_REDUNDANT_SEARCH_RADIUS) |
Filter redundant points from the point cloud. | |
void | pclPointCloudToROSPointCloud2 (const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &in, sensor_msgs::PointCloud2 &out) |
Convert a PCL point cloud to a ROS point cloud message. | |
tf2::Transform | performICP (const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &target, const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &source, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &result) |
Perform ICP on the given point clouds. | |
void | rosPointCloud2ToPCLPointCloud (const sensor_msgs::PointCloud2 in, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &out) |
Convert a ROS point cloud message to a PCL point cloud. | |
void | transformToOrigin (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &pc, const geometry_msgs::Point ¢roid) |
Transform the point cloud to be centered about the origin. | |
void | transformToOrigin (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &pc) |
Transform the point cloud to be centered about the origin. | |
void | transformToOrigin (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &pc, std::vector< graspdb::Grasp > &grasps, const geometry_msgs::Point ¢roid) |
Transform the point cloud and grasps to be centered about the origin. | |
void | transformToOrigin (const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &pc, std::vector< graspdb::Grasp > &grasps) |
Transform the point cloud and grasps to be centered about the origin. | |
Variables | |
static const int | DEFAULT_FILTER_OUTLIER_MIN_NUM_NEIGHBORS = 6 |
static const double | DEFAULT_FILTER_OUTLIER_SEARCH_RADIUS = 0.01 |
static const double | DEFAULT_FILTER_REDUNDANT_SEARCH_RADIUS = 0.0004 |
static const double | DEFAULT_METRIC_OVERLAP_SEARCH_RADIUS = 0.005 |
void rail::pick_and_place::point_cloud_metrics::calculateAvgColors | ( | const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr & | pc, |
double & | avg_r, | ||
double & | avg_g, | ||
double & | avg_b | ||
) |
Average color value calculator.
Calculate the average red, green, and blue values of the point cloud.
pc | The point cloud. |
avg_r | A reference to the average red value to store. |
avg_g | A reference to the average green value to store. |
avg_b | A reference to the average blue value to store. |
Definition at line 179 of file PointCloudMetrics.cpp.
double rail::pick_and_place::point_cloud_metrics::calculateRegistrationMetricDistanceError | ( | const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr & | base, |
const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr & | target | ||
) |
Point cloud distance error metric calculator.
Calculate the total distance between each point in target to the closest point in base.
base | The base point cloud. |
target | The target point cloud.. |
Definition at line 159 of file PointCloudMetrics.cpp.
void rail::pick_and_place::point_cloud_metrics::calculateRegistrationMetricOverlap | ( | const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr & | base, |
const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr & | target, | ||
double & | overlap, | ||
double & | color_error, | ||
const double | metric_overlap_search_radius = DEFAULT_METRIC_OVERLAP_SEARCH_RADIUS |
||
) |
Point cloud overlap metric calculator.
Calculate the overlap metric for the given point clouds. This can either be the number of points that meet the overlap criteria normalized over the number of points, or the average error in the RGB color for the overlap points. References to each double to be filled must be provided.
base | The base point cloud. |
target | The target point cloud. |
overlap | The overlap metric score. |
color_error | The color error metric score. |
metric_overlap_search_radius | The search radius to consider a point to be overlapping (defaults to constant). |
Definition at line 231 of file PointCloudMetrics.cpp.
double rail::pick_and_place::point_cloud_metrics::calculateStdDevColors | ( | const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr & | pc, |
double & | std_dev_r, | ||
double & | std_dev_g, | ||
double & | std_dev_b | ||
) |
Color value standard deviation calculator.
Calculate the red, green, and blue value standard deviations of the point cloud.
pc | The point cloud. |
std_dev_r | A reference to the standard deviation in the red value to store. |
std_dev_g | A reference to the standard deviation in the green value to store. |
std_dev_b | A reference to the standard deviation in the blue value to store. |
Definition at line 200 of file PointCloudMetrics.cpp.
double rail::pick_and_place::point_cloud_metrics::calculateStdDevColors | ( | const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr & | pc, |
double & | std_dev_r, | ||
double & | std_dev_g, | ||
double & | std_dev_b, | ||
const double | avg_r, | ||
const double | avg_g, | ||
const double | avg_b | ||
) |
Color value standard deviation calculator.
Calculate the red, green, and blue value standard deviations of the point cloud with the known averages.
pc | The point cloud. |
std_dev_r | A reference to the standard deviation in the red value to store. |
std_dev_g | A reference to the standard deviation in the green value to store. |
std_dev_b | A reference to the standard deviation in the blue value to store. |
avg_r | The average red color. |
avg_g | The average green color. |
avg_b | The average blue color. |
Definition at line 209 of file PointCloudMetrics.cpp.
bool rail::pick_and_place::point_cloud_metrics::classifyMerge | ( | const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr & | base, |
const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr & | target | ||
) |
Classify the point cloud merge.
Classify the merge between base and target. These point clouds should already be transformed accordingly. The decision is based on values from a trained decision tree on point cloud metrics.
base | The base point cloud. |
target | The target point cloud. |
Definition at line 273 of file PointCloudMetrics.cpp.
geometry_msgs::Point rail::pick_and_place::point_cloud_metrics::computeCentroid | ( | const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr & | pc | ) |
Compute the centroid of the given point cloud.
Compute and return the centroid of the given point cloud.
pc | The point cloud to compute the centroid of. |
Definition at line 146 of file PointCloudMetrics.cpp.
void rail::pick_and_place::point_cloud_metrics::filterPointCloudOutliers | ( | const pcl::PointCloud< pcl::PointXYZRGB >::Ptr & | pc, |
const double | filter_outlier_search_radius = DEFAULT_FILTER_OUTLIER_SEARCH_RADIUS , |
||
const double | filter_outlier_min_num_neighbors = DEFAULT_FILTER_OUTLIER_MIN_NUM_NEIGHBORS |
||
) |
Filter point cloud outliers.
Filter outliers in the point cloud.
pc | The point cloud to remove outlier points from. |
filter_outlier_search_radius | The search radius to be considered as an outlier point (defaults to constant). |
filter_outlier_min_num_neighbors | The minimum neighbors to be consider as an outlier (defaults to constant). |
Definition at line 92 of file PointCloudMetrics.cpp.
void rail::pick_and_place::point_cloud_metrics::filterRedundantPoints | ( | const pcl::PointCloud< pcl::PointXYZRGB >::Ptr & | pc, |
const double | filter_redundant_search_radius = DEFAULT_FILTER_REDUNDANT_SEARCH_RADIUS |
||
) |
Filter redundant points from the point cloud.
Filter redundant points from the point cloud effectively downsampling it.
pc | The point cloud to remove redundant points from. |
filter_redundant_search_radius | The search radius to be considered as a redundant point (defaults to constant). |
Definition at line 120 of file PointCloudMetrics.cpp.
void rail::pick_and_place::point_cloud_metrics::pclPointCloudToROSPointCloud2 | ( | const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr & | in, |
sensor_msgs::PointCloud2 & | out | ||
) |
Convert a PCL point cloud to a ROS point cloud message.
Converts the given PCL point cloud to a ROS point cloud message.
in | The input PCL point cloud message. |
out | The ROS point cloud message to create. |
Definition at line 35 of file PointCloudMetrics.cpp.
tf2::Transform rail::pick_and_place::point_cloud_metrics::performICP | ( | const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr & | target, |
const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr & | source, | ||
const pcl::PointCloud< pcl::PointXYZRGB >::Ptr & | result | ||
) |
Perform ICP on the given point clouds.
Perform ICP on the given point clouds and store the resulting transformed point cloud in the result pointer.
target | The target point cloud. |
source | The source point cloud (to transform to the target). |
result | The transformed source point cloud. |
Definition at line 289 of file PointCloudMetrics.cpp.
void rail::pick_and_place::point_cloud_metrics::rosPointCloud2ToPCLPointCloud | ( | const sensor_msgs::PointCloud2 | in, |
const pcl::PointCloud< pcl::PointXYZRGB >::Ptr & | out | ||
) |
Convert a ROS point cloud message to a PCL point cloud.
Converts the given ROS point cloud message to a PCL point cloud.
in | The input ROS point cloud message. |
out | The PCL point cloud to create. |
Definition at line 27 of file PointCloudMetrics.cpp.
void rail::pick_and_place::point_cloud_metrics::transformToOrigin | ( | const pcl::PointCloud< pcl::PointXYZRGB >::Ptr & | pc, |
const geometry_msgs::Point & | centroid | ||
) |
Transform the point cloud to be centered about the origin.
Transform the point cloud to be centered about the origin based on the given centroid of the point cloud.
pc | The point cloud to transform. |
centroid | The known centroid of the point cloud. |
Definition at line 45 of file PointCloudMetrics.cpp.
void rail::pick_and_place::point_cloud_metrics::transformToOrigin | ( | const pcl::PointCloud< pcl::PointXYZRGB >::Ptr & | pc | ) |
Transform the point cloud to be centered about the origin.
Transform the point cloud to be centered about the origin based on the centroid of the point cloud.
pc | The point cloud to transform. |
Definition at line 59 of file PointCloudMetrics.cpp.
void rail::pick_and_place::point_cloud_metrics::transformToOrigin | ( | const pcl::PointCloud< pcl::PointXYZRGB >::Ptr & | pc, |
std::vector< graspdb::Grasp > & | grasps, | ||
const geometry_msgs::Point & | centroid | ||
) |
Transform the point cloud and grasps to be centered about the origin.
Transform the point cloud and grasps to be centered about the origin based on the given centroid of the point cloud.
pc | The point cloud to transform. |
grasps | The grasps to transform. |
centroid | The known centroid of the point cloud. |
void rail::pick_and_place::point_cloud_metrics::transformToOrigin | ( | const pcl::PointCloud< pcl::PointXYZRGB >::Ptr & | pc, |
std::vector< graspdb::Grasp > & | grasps | ||
) |
Transform the point cloud and grasps to be centered about the origin.
Transform the point cloud and grasps to be centered about the origin based on the centroid of the point cloud.
pc | The point cloud to transform. |
grasps | The grasps to transform. |
const int rail::pick_and_place::point_cloud_metrics::DEFAULT_FILTER_OUTLIER_MIN_NUM_NEIGHBORS = 6 [static] |
The minimum number of neighbors required to keep a point during pre-processing.
Definition at line 40 of file PointCloudMetrics.h.
const double rail::pick_and_place::point_cloud_metrics::DEFAULT_FILTER_OUTLIER_SEARCH_RADIUS = 0.01 [static] |
The radius to search within for neighbors during pre-processing.
Definition at line 36 of file PointCloudMetrics.h.
const double rail::pick_and_place::point_cloud_metrics::DEFAULT_FILTER_REDUNDANT_SEARCH_RADIUS = 0.0004 [static] |
The radius to search within for neighbors during pre-processing.
Definition at line 38 of file PointCloudMetrics.h.
const double rail::pick_and_place::point_cloud_metrics::DEFAULT_METRIC_OVERLAP_SEARCH_RADIUS = 0.005 [static] |
The radius to search within for neighbors during the overlap metric search.
Definition at line 42 of file PointCloudMetrics.h.