/*! @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;
}
