/*! @brief the main processing callback of the ECTO pipeline
*
* this method is called once all input dependencies are satisfied.
* The PartsBasedDetector has two input dependencies: a color image and depth image,
* both retrieved from the Kinect. If any detection candidates are found,
* the bounding boxes, detection confidences and object ids are returned
*
* @param inputs the input tendrils
* @param outputs the output tendrils
* @return
*/
int process(const tendrils& inputs, const tendrils& outputs)
{
std::cout << "detector: process" << std::endl;
pose_results_->clear();
image_pipeline::PinholeCameraModel camera_model;
camera_model.setParams(color_->size(), *camera_intrinsics_, cv::Mat(),
cv::Mat(), cv::Mat());
std::vector<Candidate> candidates;
detector_->detect(*color_, *depth_, candidates);
if (candidates.size() == 0)
{
if (*visualize_)
{
cv::cvtColor(*color_, *output_, CV_RGB2BGR);
//cv::waitKey(30);
}
return ecto::OK;
}
Candidate::sort(candidates);
Candidate::nonMaximaSuppression(*color_, candidates, *max_overlap_);
if (*visualize_)
{
visualizer_->candidates(*color_, candidates, candidates.size(), *output_, true);
cv::cvtColor(*output_, *output_, CV_RGB2BGR);
//cv::waitKey(30);
}
std::vector<Rect3d> bounding_boxes;
std::vector<PointCloud> parts_centers;
typename PointCloudClusterer<PointType>::PointProjectFunc projecter =
boost::bind(&PartsBasedDetectorCell::projectPixelToRay, this,
camera_model, _1);
PointCloudClusterer<PointType>::computeBoundingBoxes(candidates,
*color_, *depth_, projecter, *input_cloud_, bounding_boxes,
parts_centers);
// output clusters
std::vector<PointType> object_centers;
std::vector<PointCloud> clusters;
// remove planes from input cloud if needed
if(*remove_planes_)
{
PointCloud::Ptr clusterer_cloud (new PointCloud());
PointCloudClusterer<PointType>::organizedMultiplaneSegmentation(*input_cloud_, *clusterer_cloud);
PointCloudClusterer<PointType>::clusterObjects(clusterer_cloud,
bounding_boxes, clusters, object_centers);
}
else
{
PointCloudClusterer<PointType>::clusterObjects(*input_cloud_,
bounding_boxes, clusters, object_centers);
}
// compute poses (centroid of part centers)
// for each object
for (int object_it = 0; object_it < candidates.size(); ++object_it)
{
if(std::isnan(object_centers[object_it].x) || std::isnan(object_centers[object_it].y) || std::isnan(object_centers[object_it].z))
continue;
PoseResult result;
// no db for now, only one model
result.set_object_id(*object_db_, model_name_);
result.set_confidence(candidates[object_it].score());
// set the clustered cloud's center as a center...
result.set_T(Eigen::Vector3f(object_centers[object_it].getVector3fMap()));
// // For the rotation a minimum of two parts is needed
// if (part_centers_cloud.size() >= 2 &&
// !pcl_isnan(part_centers_cloud[0].x) &&
// !pcl_isnan(part_centers_cloud[0].y) &&
// !pcl_isnan(part_centers_cloud[0].z) &&
// !pcl_isnan(part_centers_cloud[1].x) &&
// !pcl_isnan(part_centers_cloud[1].y) &&
// !pcl_isnan(part_centers_cloud[1].z))
// {
// Eigen::Vector3f center(centroid.block<3, 1>(0, 0));
//
// Eigen::Vector3f x_axis(
// part_centers_cloud[0].getVector3fMap() - center);
// x_axis.normalize();
// Eigen::Vector3f z_axis =
// (x_axis.cross(
// part_centers_cloud[1].getVector3fMap() - center)).normalized();
//
// Eigen::Vector3f y_axis = x_axis.cross(z_axis); // should be normalized
//
// Eigen::Matrix3f rot_matr;
// rot_matr << z_axis, y_axis, -x_axis;
// //rot_matr.transposeInPlace();
//
// result.set_R(rot_matr);
// }
// else
{
result.set_R(Eigen::Quaternionf(1, 0, 0, 0));
}
// Only one point of view for this object...
sensor_msgs::PointCloud2Ptr cluster_cloud (new sensor_msgs::PointCloud2());
std::vector<sensor_msgs::PointCloud2ConstPtr> ros_clouds (1);
pcl::toROSMsg(clusters[object_it], *(cluster_cloud));
ros_clouds[0] = cluster_cloud;
result.set_clouds(ros_clouds);
std::vector<PointCloud, Eigen::aligned_allocator<PointCloud> > object_cluster (1);
object_cluster[0] = clusters[object_it];
pose_results_->push_back(result);
}
return ecto::OK;
}
/** Compute the pose of the table plane
* @param inputs
* @param outputs
* @return
*/
int
process(const tendrils& inputs, const tendrils& outputs)
{
std::vector<tabletop_object_detector::TabletopObjectRecognizer<pcl::PointXYZ>::TabletopResult > results;
// Process each table
pose_results_->clear();
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clusters_merged;
// Map to store the transformation for each cluster (table_index)
std::map<pcl::PointCloud<pcl::PointXYZ>::Ptr, size_t> cluster_table;
std::vector<cv::Vec3f> translations(clusters_->size());
std::vector<cv::Matx33f> rotations(clusters_->size());
for (size_t table_index = 0; table_index < clusters_->size(); ++table_index)
{
getPlaneTransform((*table_coefficients_)[table_index], rotations[table_index], translations[table_index]);
// Make the clusters be in the table frame
size_t n_clusters = (*clusters_)[table_index].size();
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clusters(n_clusters);
cv::Matx33f Rinv = rotations[table_index].t();
cv::Vec3f Tinv = -Rinv*translations[table_index];
for (size_t cluster_index = 0; cluster_index < n_clusters; ++cluster_index)
{
clusters[cluster_index] = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
for(size_t i = 0; i < (*clusters_)[table_index][cluster_index].size(); ++i)
{
cv::Vec3f res = Rinv*(*clusters_)[table_index][cluster_index][i] + Tinv;
clusters[cluster_index]->push_back(pcl::PointXYZ(res[0], res[1], res[2]));
}
cluster_table.insert(std::pair<pcl::PointCloud<pcl::PointXYZ>::Ptr, size_t>(clusters[cluster_index], table_index));
}
clusters_merged.insert(clusters_merged.end(), clusters.begin(), clusters.end());
}
object_recognizer_.objectDetection(clusters_merged, confidence_cutoff_, perform_fit_merge_, results);
for (size_t i = 0; i < results.size(); ++i)
{
const tabletop_object_detector::TabletopObjectRecognizer<pcl::PointXYZ>::TabletopResult & result = results[i];
const size_t table_index = cluster_table[result.cloud_];
PoseResult pose_result;
// Add the object id
std::stringstream ss;
ss << result.object_id_;
pose_result.set_object_id(db_, ss.str());
// Add the pose
const geometry_msgs::Pose &pose = result.pose_;
cv::Vec3f T(pose.position.x, pose.position.y, pose.position.z);
Eigen::Quaternionf quat(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z);
cv::Vec3f new_T = rotations[table_index] * T + translations[table_index];
pose_result.set_T(cv::Mat(new_T));
pose_result.set_R(quat);
cv::Mat R = cv::Mat(rotations[table_index] * pose_result.R<cv::Matx33f>());
pose_result.set_R(R);
pose_result.set_confidence(result.confidence_);
// Add the cluster of points
std::vector<sensor_msgs::PointCloud2ConstPtr> ros_clouds (1);
sensor_msgs::PointCloud2Ptr cluster_cloud (new sensor_msgs::PointCloud2());
#if PCL_VERSION_COMPARE(>=,1,7,0)
::pcl::PCLPointCloud2 pcd_tmp;
::pcl::toPCLPointCloud2(*result.cloud_, pcd_tmp);
pcl_conversions::fromPCL(pcd_tmp, *cluster_cloud);
#else
pcl::toROSMsg(*result.cloud_, *cluster_cloud);
#endif
ros_clouds[0] = cluster_cloud;
pose_result.set_clouds(ros_clouds);
pose_results_->push_back(pose_result);
}
return ecto::OK;
}
void Cup_Segmentation::cloudCallback (const sensor_msgs::PointCloud2::ConstPtr& cloud_in)
{
ROS_INFO("Processing!");
/************************ CENTER AND LEFT BOXES ***************************************/
//Creating point cloud and convert ROS Message
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PointCloud<pcl::PointXYZ>::Ptr seg_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*cloud_in, *pcl_cloud);
//Create and define filter parameters
pcl::PassThrough<pcl::PointXYZ> pass3;
pass3.setFilterFieldName("x");
pass3.setFilterLimits(0, 1.2); //-0.5 0.5
pass3.setInputCloud(pcl_cloud);
pass3.filter(*pcl_cloud);
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setFilterFieldName("y");
pass.setFilterLimits(-0.7, 0.1); //-0.5 0.5
pass.setInputCloud(pcl_cloud);
pass.filter(*pcl_cloud);
pcl::PassThrough<pcl::PointXYZ> pass2;
pass2.setFilterFieldName("z");
pass2.setFilterLimits(0.0, 1.0); //-0.5 0.5
pass2.setInputCloud(pcl_cloud);
pass2.filter(*pcl_cloud);
//Model fitting process ->RANSAC
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PARALLEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.03); //0.03
seg.setAxis (Eigen::Vector3f(1, 0, 0));
seg.setEpsAngle (0.1); //0.02
seg.setInputCloud (pcl_cloud);
seg.segment (*inliers, *coefficients);
//Verify if inliers is not empty
if (inliers->indices.size () == 0)
return;
pcl::PointCloud<pcl::PointXYZ>::Ptr treated_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
for (std::vector<int>::const_iterator it = inliers->indices.begin(); it != inliers->indices.end (); ++it)
treated_cloud->push_back(pcl_cloud->points[*it]);
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(pcl_cloud);
extract.setIndices(inliers);
extract.setNegative(true);
extract.filter(*seg_cloud);
//Create and define radial filter parameters
//pcl::RadiusOutlierRemoval<pcl::PointXYZ> radialFilter;
//radialFilter.setInputCloud(treated_cloud);
//radialFilter.setRadiusSearch(0.03);
//radialFilter.setMinNeighborsInRadius (20);
//radialFilter.filter (*treated_cloud);
//Apply clustering algorithm
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree (new pcl::search::KdTree<pcl::PointXYZ>);
kdtree->setInputCloud (seg_cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.06);
ec.setMinClusterSize (6);
ec.setMaxClusterSize (150);
ec.setSearchMethod (kdtree);
ec.setInputCloud (seg_cloud);
ec.extract (cluster_indices);
pcl::PointCloud<pcl::PointXYZI>::Ptr cluster_cloud (new pcl::PointCloud<pcl::PointXYZI> ());
pcl::PointXYZI cluster_point;
double cluster_final_average;
int cluster_id=0;
float x1 = 0.0, y1 = 0.0, z1 = 0.0;
float x2 = 0.0, y2 = 0.0, z2 = 0.0;
float x3 = 0.0, y3 = 0.0, z3 = 0.0;
int total1 = 0, total2 = 0, total3 = 0;
bool hasCup1, hasCup2, hasCup3;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end ();
++it, cluster_id+=1)
{
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
{
cluster_point.x = seg_cloud->points[*pit].x;
cluster_point.y = seg_cloud->points[*pit].y;
cluster_point.z = seg_cloud->points[*pit].z;
cluster_point.intensity = cluster_id;
cluster_cloud->push_back(cluster_point);
if(cluster_id == 0){
x1 += seg_cloud->points[*pit].x;
y1 += seg_cloud->points[*pit].y;
z1 += seg_cloud->points[*pit].z;
total1++;
} else if (cluster_id == 1){
x2 += seg_cloud->points[*pit].x;
y2 += seg_cloud->points[*pit].y;
z2 += seg_cloud->points[*pit].z;
total2++;
} else if (cluster_id == 2){
x3 += seg_cloud->points[*pit].x;
y3 += seg_cloud->points[*pit].y;
z3 += seg_cloud->points[*pit].z;
total3++;
}
}
}
if(total1 != 0 ){
x1 = x1/total1;
y1 = y1/total1;
z1 = z1/total1;
hasCup1=true;
}else{
hasCup1 = false;
}
if(total2 != 0 ){
x2 = x2/total2;
y2 = y2/total2;
z2 = z2/total2;
hasCup2 = true;
} else {
hasCup2 = false;
}
if(total3 != 0){
x3 = x3/total2;
y3 = y3/total2;
z3 = z3/total2;
hasCup3 = true;
} else{
hasCup3 = false;
}
//Publish message
sensor_msgs::PointCloud2 cloud;
pcl::toROSMsg(*cluster_cloud, cloud);
cloud.header.stamp = ros::Time::now();
cloud.header.frame_id = cloud_in->header.frame_id;
treated_cloud_pub_.publish(cloud);
//Geometry
geometry_msgs::PointStamped cupPoint1;
geometry_msgs::PointStamped cupPoint2;
geometry_msgs::PointStamped cupPoint3;
tf::Quaternion q;
geometry_msgs::PointStamped cupSensorPoint1;
geometry_msgs::PointStamped cupSensorPoint2;
geometry_msgs::PointStamped cupSensorPoint3;
static tf::TransformBroadcaster tfBc1;
static tf::TransformBroadcaster tfBc2;
static tf::TransformBroadcaster tfBc3;
tf::Transform tfCup1;
tf::Transform tfCup2;
tf::Transform tfCup3;
cupPoint1.header.frame_id = "base_footprint";
cupPoint2.header.frame_id = "base_footprint";
cupPoint3.header.frame_id = "base_footprint";
cupPoint1.header.stamp = cloud_in->header.stamp;
cupPoint2.header.stamp = cloud_in->header.stamp;
cupPoint3.header.stamp = cloud_in->header.stamp;
cupPoint1.point.x = x1;
cupPoint2.point.x = x2;
cupPoint3.point.x = x3;
cupPoint1.point.y = y1;
cupPoint2.point.y = y2;
cupPoint3.point.y = y3;
cupPoint1.point.z = z1;
cupPoint2.point.z = z2;
cupPoint3.point.z = z3;
ROS_INFO("Cup 1: X=%f Y=%f Z=%f", cupPoint1.point.x, cupPoint1.point.y, cupPoint1.point.z);
ROS_INFO("Cup 2: X=%f Y=%f Z=%f", cupPoint2.point.x, cupPoint2.point.y, cupPoint2.point.z);
ROS_INFO("Cup 3: X=%f Y=%f Z=%f", cupPoint3.point.x, cupPoint3.point.y, cupPoint3.point.z);
tf_listener.transformPoint("sensors_frame", cupPoint1, cupSensorPoint1);
tf_listener.transformPoint("sensors_frame", cupPoint2, cupSensorPoint2);
tf_listener.transformPoint("sensors_frame", cupPoint3, cupSensorPoint3);
tfCup1.setOrigin( tf::Vector3(cupSensorPoint1.point.x, cupSensorPoint1.point.y, cupSensorPoint1.point.z) );
tfCup2.setOrigin( tf::Vector3(cupSensorPoint2.point.x, cupSensorPoint2.point.y, cupSensorPoint2.point.z) );
tfCup3.setOrigin( tf::Vector3(cupSensorPoint3.point.x, cupSensorPoint3.point.y, cupSensorPoint3.point.z) );
ROS_INFO("Sensor Frame Cup 1: X=%f Y=%f Z=%f", cupSensorPoint1.point.x, cupSensorPoint1.point.y, cupSensorPoint1.point.z);
ROS_INFO("Sensor Frame Cup 2: X=%f Y=%f Z=%f", cupSensorPoint2.point.x, cupSensorPoint2.point.y, cupSensorPoint2.point.z);
ROS_INFO("Sensor Frame Cup 3: X=%f Y=%f Z=%f", cupSensorPoint3.point.x, cupSensorPoint3.point.y, cupSensorPoint3.point.z);
q.setRPY(0, 0, 0);
tfCup1.setRotation(q);
tfCup2.setRotation(q);
tfCup3.setRotation(q);
tfBc1.sendTransform(tf::StampedTransform(tfCup1, cloud_in->header.stamp, "sensors_frame", "cup1"));
tfBc2.sendTransform(tf::StampedTransform(tfCup2, cloud_in->header.stamp, "sensors_frame", "cup2"));
tfBc3.sendTransform(tf::StampedTransform(tfCup3, cloud_in->header.stamp, "sensors_frame", "cup3"));
ROS_INFO("All Sent!");
}
void
PrimitiveShapeClassifier::process(const sensor_msgs::PointCloud2::ConstPtr& ros_cloud,
const sensor_msgs::PointCloud2::ConstPtr& ros_normal,
const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& ros_indices,
const jsk_recognition_msgs::PolygonArray::ConstPtr& ros_polygons)
{
boost::mutex::scoped_lock lock(mutex_);
if (!checkFrameId(ros_cloud, ros_normal, ros_indices, ros_polygons)) return;
pcl::PointCloud<PointT>::Ptr input(new pcl::PointCloud<PointT>);
pcl::fromROSMsg(*ros_cloud, *input);
pcl::PointCloud<pcl::Normal>::Ptr normal(new pcl::PointCloud<pcl::Normal>);
pcl::fromROSMsg(*ros_normal, *normal);
pcl::ExtractIndices<PointT> ext_input;
ext_input.setInputCloud(input);
pcl::ExtractIndices<pcl::Normal> ext_normal;
ext_normal.setInputCloud(normal);
std::vector<jsk_recognition_utils::Polygon::Ptr> polygons
= jsk_recognition_utils::Polygon::fromROSMsg(*ros_polygons);
jsk_recognition_msgs::ClassificationResult result;
result.header = ros_cloud->header;
result.classifier = "primitive_shape_classifier";
result.target_names.push_back("box");
result.target_names.push_back("circle");
result.target_names.push_back("other");
pcl::PointCloud<PointT>::Ptr projected_cloud(new pcl::PointCloud<PointT>);
std::vector<pcl::PointIndices::Ptr> boundary_indices;
NODELET_DEBUG_STREAM("Cluster num: " << ros_indices->cluster_indices.size());
for (size_t i = 0; i < ros_indices->cluster_indices.size(); ++i)
{
pcl::PointIndices::Ptr indices(new pcl::PointIndices);
indices->indices = ros_indices->cluster_indices[i].indices;
NODELET_DEBUG_STREAM("Estimating cluster #" << i << " (" << indices->indices.size() << " points)");
pcl::PointCloud<PointT>::Ptr cluster_cloud(new pcl::PointCloud<PointT>);
ext_input.setIndices(indices);
ext_input.filter(*cluster_cloud);
pcl::PointCloud<pcl::Normal>::Ptr cluster_normal(new pcl::PointCloud<pcl::Normal>);
ext_normal.setIndices(indices);
ext_normal.filter(*cluster_normal);
pcl::ModelCoefficients::Ptr support_plane(new pcl::ModelCoefficients);
if (!getSupportPlane(cluster_cloud, polygons, support_plane))
{
NODELET_ERROR_STREAM("cloud " << i << " has no support plane. skipped");
continue;
}
pcl::PointIndices::Ptr b(new pcl::PointIndices);
pcl::PointCloud<PointT>::Ptr pc(new pcl::PointCloud<PointT>);
float circle_likelihood, box_likelihood;
estimate(cluster_cloud, cluster_normal, support_plane,
b, pc,
circle_likelihood, box_likelihood);
boundary_indices.push_back(std::move(b));
*projected_cloud += *pc;
if (circle_likelihood > circle_threshold_) {
// circle
result.labels.push_back(1);
result.label_names.push_back("circle");
result.label_proba.push_back(circle_likelihood);
} else if (box_likelihood > box_threshold_) {
// box
result.labels.push_back(0);
result.label_names.push_back("box");
result.label_proba.push_back(box_likelihood);
} else {
// other
result.labels.push_back(3);
result.label_names.push_back("other");
result.label_proba.push_back(0.0);
}
}
// publish results
sensor_msgs::PointCloud2 ros_projected_cloud;
pcl::toROSMsg(*projected_cloud, ros_projected_cloud);
ros_projected_cloud.header = ros_cloud->header;
pub_projected_cloud_.publish(ros_projected_cloud);
jsk_recognition_msgs::ClusterPointIndices ros_boundary_indices;
ros_boundary_indices.header = ros_cloud->header;
for (size_t i = 0; i < boundary_indices.size(); ++i)
{
pcl_msgs::PointIndices ri;
pcl_conversions::moveFromPCL(*boundary_indices[i], ri);
ros_boundary_indices.cluster_indices.push_back(ri);
}
pub_boundary_indices_.publish(ros_boundary_indices);
pub_class_.publish(result);
}
