/*! @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;
std::vector<Candidate> candidates;
detector_->detect(*color_, *depth_, candidates);
if (true) {
if (candidates.size() > 0) {
Candidate::sort(candidates);
visualizer_->candidates(*color_, candidates, 1, *output_, true);
} else {
cvtColor(*color_, *output_, CV_RGB2BGR);
}
cv::waitKey(30);
}
pose_results_->clear();
return ecto::OK;
}
/*! @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;
}
