PointCloudMetrics.h

Go to the documentation of this file.

#ifndef RAIL_PICK_AND_PLACE_POINT_CLOUD_METRICS_H_
#define RAIL_PICK_AND_PLACE_POINT_CLOUD_METRICS_H_

// ROS
#include <geometry_msgs/Point.h>
#include <graspdb/Grasp.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf2/LinearMath/Transform.h>

// PCL
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

// C++ Standard Library
#include <vector>

namespace rail
{
namespace pick_and_place
{
namespace point_cloud_metrics
{

static const double DEFAULT_FILTER_OUTLIER_SEARCH_RADIUS = 0.01;
static const double DEFAULT_FILTER_REDUNDANT_SEARCH_RADIUS = 0.0004;
static const int DEFAULT_FILTER_OUTLIER_MIN_NUM_NEIGHBORS = 6;
static const double DEFAULT_METRIC_OVERLAP_SEARCH_RADIUS = 0.005;

void rosPointCloud2ToPCLPointCloud(const sensor_msgs::PointCloud2 in,
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &out);

void pclPointCloudToROSPointCloud2(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in,
    sensor_msgs::PointCloud2 &out);

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);

void filterRedundantPoints(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pc,
    const double filter_redundant_search_radius = DEFAULT_FILTER_REDUNDANT_SEARCH_RADIUS);

geometry_msgs::Point computeCentroid(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &pc);

void transformToOrigin(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pc, const geometry_msgs::Point &centroid);

void transformToOrigin(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pc);

void transformToOrigin(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pc,
    std::vector<graspdb::Grasp> &grasps, const geometry_msgs::Point &centroid);

void transformToOrigin(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pc,
    std::vector<graspdb::Grasp> &grasps);

double calculateRegistrationMetricDistanceError(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &base,
    const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &target);

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);

void calculateAvgColors(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &pc, double &avg_r, double &avg_g,
    double &avg_b);

double calculateStdDevColors(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &pc, double &std_dev_r,
    double &std_dev_g, double &std_dev_b);

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);

bool classifyMerge(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &base,
    const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &target);

tf2::Transform performICP(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &target,
    const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &source, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &result);
}
}
}

#endif