zyonz_find_leaf_probing_points_alg_node.cpp

Go to the documentation of this file.

#include "zyonz_find_leaf_probing_points_alg_node.h"

/*  [sorting function] */
bool MyDataSortPredicate(const geometry_msgs::Pose& d1, const geometry_msgs::Pose& d2)
{
  return d1.position.y < d2.position.y;
}

ZyonzFindLeafProbingPointsAlgNode::ZyonzFindLeafProbingPointsAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<ZyonzFindLeafProbingPointsAlgorithm>(),
  data_processed_(false)
{
  //init class attributes if necessary
  //this->loop_rate_ = 2;//in [Hz]

  // [init publishers]
  poses_publisher_ = public_node_handle_.advertise<geometry_msgs::PoseArray>("poses", 10);
  output_marker_publisher_ = public_node_handle_.advertise<visualization_msgs::Marker>("output_marker", 10);
  output_pointcloud_publisher_ = public_node_handle_.advertise<sensor_msgs::PointCloud2>("output_pointcloud", 10);
  
  // [init subscribers]
  input_subscriber_ = public_node_handle_.subscribe("input", 10, &ZyonzFindLeafProbingPointsAlgNode::input_callback, this);
  
  // [init services]
  get_pose_array_server_ = public_node_handle_.advertiseService("get_pose_array", &ZyonzFindLeafProbingPointsAlgNode::get_pose_arrayCallback, this);
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
}

ZyonzFindLeafProbingPointsAlgNode::~ZyonzFindLeafProbingPointsAlgNode(void)
{
  // [free dynamic memory]
}

void ZyonzFindLeafProbingPointsAlgNode::mainNodeThread(void)
{
  // [fill msg structures]
  //this->PoseArray_msg_.data = my_var;
  //this->Marker_msg_.data = my_var;
  //this->PointCloud2_msg_.data = my_var;
  
  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]

  // [publish messages]
  //this->poses_publisher_.publish(this->PoseArray_msg_);
  //this->output_marker_publisher_.publish(this->Marker_msg_);
  //this->output_pointcloud_publisher_.publish(this->PointCloud2_msg_);
}

/*  [subscriber callbacks] */
void ZyonzFindLeafProbingPointsAlgNode::input_callback(const sensor_msgs::PointCloud2::ConstPtr& msg) 
{ 
  //ROS_INFO("ZyonzFindLeafProbingPointsAlgNode::input_callback: New Message Received"); 
  
  /* Load input PointCloud2 */
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromROSMsg (*msg, *cloud);

  //use appropiate mutex to shared variables if necessary 
  //alg_.lock(); 
  //input_mutex_.enter(); 
  
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  // Optional
  seg.setOptimizeCoefficients (true);
  // Mandatory
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold (0.01);
  seg.setInputCloud (cloud);
  seg.segment (*inliers, *coefficients);
//  std::cerr << "PointCloud after segmentation has: "
//            << inliers->indices.size () << " inliers." << std::endl;
  
  // Project the model inliers 
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::ProjectInliers<pcl::PointXYZ> proj;
  proj.setModelType (pcl::SACMODEL_PLANE);
  proj.setInputCloud (cloud);
  proj.setModelCoefficients (coefficients);
  proj.filter (*cloud_projected);
//  std::cerr << "PointCloud after projection has: "
//            << cloud_projected->points.size () << " data points." << std::endl;
  
// Create a Concave Hull representation of the projected inliers
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::ConcaveHull<pcl::PointXYZ> chull;
  //  chull.setKeepInformation (true);
  chull.setInputCloud (cloud_projected);
  chull.setAlpha (0.1);
  chull.reconstruct (*cloud_hull);
  
  // Compute the cluster centroid
  Eigen::Vector4f centroid;
  pcl::compute3DCentroid (*cloud, centroid);
  // ROS_INFO("Cluster Centroid: (%f, %f, %f)", centroid(0), centroid(1), centroid(2));
  
  // Principal Component Analysis (PCA)
  // 1) Compute Covariance
  Eigen::MatrixXf B(cloud->points.size(),3);
  int aa = 0;
  for (pcl::PointCloud<pcl::PointXYZ>::iterator it = cloud->begin(); it != cloud->end(); ++it, ++aa){
    B.row(aa)[0] = it->x - centroid(0);
    B.row(aa)[1] = it->y - centroid(1);
    B.row(aa)[2] = it->z - centroid(2);
    // std::cout << "Point " << aa << " of " << cloud_filtered->points.size() << ": " << it->x << ", " << it->y << ", " << it->z << std::endl;
  }
  Eigen::Matrix3f C = (B.transpose()*B)/cloud->points.size();
  //std::cout << "Covariance Matrix: \n" << C << std::endl;
  
  // 2) Get EigenValues and EigenVectors
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver(C);
  if (eigensolver.info() != Eigen::Success)
    ROS_ERROR("EigenVectors not found :'(");
  std::cout << "Eigenvalues are: \n" << eigensolver.eigenvalues() << std::endl << "Eigenvectors are: \n" << eigensolver.eigenvectors() << std::endl;
  std::cout << "1st EigenVector: (" << eigensolver.eigenvectors()(0,0) << ", " << eigensolver.eigenvectors()(0,1) << ", " << eigensolver.eigenvectors()(0,2) << ")" << std::endl; 
  
  //TODO: Check the direction of the Z axis respect the camera
  Eigen::Matrix3f rot_m;
  rot_m << -eigensolver.eigenvectors()(0,1), -eigensolver.eigenvectors()(0,2), eigensolver.eigenvectors()(0,0),
           -eigensolver.eigenvectors()(1,1), -eigensolver.eigenvectors()(1,2), eigensolver.eigenvectors()(1,0),
           -eigensolver.eigenvectors()(2,1), -eigensolver.eigenvectors()(2,2), eigensolver.eigenvectors()(2,0);
  
  //Eigen::Quaternionf pose_c(eigensolver.eigenvectors());
  Eigen::Quaternionf pose_c(rot_m);
  // std::cout << "Quaternion: \n" << "(" << pose_c.x() << ", " << pose_c.y() << ", " << pose_c.z() << ", " << pose_c.w() << ")" << std::endl;
  
  // Transform (translate and rotate) the countours to the origen of the world so we have the leaf correctly oriented
  Eigen::Vector3f translation (-centroid(0), -centroid(1), -centroid(2));
  pcl::PointCloud<pcl::PointXYZ>::Ptr trasl_cluster (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr trasl_rot_cluster (new pcl::PointCloud<pcl::PointXYZ>);
  // I apply the transformation separately because first rotates and then translates :(  
  pcl::transformPointCloud(*cloud_hull, *trasl_cluster, translation, Eigen::Quaternionf(1, 0, 0, 0));
  pcl::transformPointCloud(*trasl_cluster, *trasl_rot_cluster, Eigen::Vector3f(0,0,0), pose_c.conjugate());
  
  // Extracting the right side indices
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster_right (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PassThrough<pcl::PointXYZ> pass2;
  pass2.setInputCloud (trasl_rot_cluster);
  pass2.setFilterFieldName ("x");
  pass2.setFilterLimits (0, +20.0);
  pass2.filter (*cloud_cluster_right);
  //  ROS_INFO("PointCloud after passthrough filtering has: %u data points", (unsigned int)cloud_cluster_right->size());
  
  // Downsample the contour
  pcl::VoxelGrid<pcl::PointXYZ> sor;
  sor.setInputCloud (cloud_cluster_right);
  sor.setLeafSize (0.1f, 0.1f, 0.1f);
  sor.filter (*cloud_cluster_right);
  
  // Reduce 2 cm distance to the center
  for (pcl::PointCloud<pcl::PointXYZ>::iterator it = cloud_cluster_right->begin(); it != cloud_cluster_right->end(); ++it){
    Eigen::Vector3f point(it->x, it->y, it->z);
    it->x = point.x()*(point.norm()-0.02)/point.norm();
    it->y = point.y()*(point.norm()-0.02)/point.norm();
    it->z = point.z()*(point.norm()-0.02)/point.norm();
  }
  
  // Transform (translate and rotate) the countours to the origen of the world so we have the leaf correctly oriented
  translation = Eigen::Vector3f(centroid(0), centroid(1), centroid(2));
  // I apply the transformation separately because first rotates and then translates :(  
  pcl::transformPointCloud(*cloud_cluster_right, *trasl_cluster, Eigen::Vector3f(0, 0, 0), pose_c);
  pcl::transformPointCloud(*trasl_cluster, *cloud_cluster_right, translation, Eigen::Quaternionf(1, 0, 0, 0));
  
  static tf::TransformBroadcaster br;
  tf::Transform transform;
  transform.setOrigin( tf::Vector3(centroid(0), centroid(1), centroid(2)) );
  transform.setRotation( tf::Quaternion(pose_c.x(), pose_c.y(), pose_c.z(), pose_c.w()) );
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/camboard_frame", "centroid"));

//  uint32_t shape = visualization_msgs::Marker::CUBE;
  uint32_t shape = visualization_msgs::Marker::SPHERE;
//  uint32_t shape = visualization_msgs::Marker::ARROW;
//  uint32_t shape = visualization_msgs::Marker::CYLINDER;
  Marker_msg_.header.frame_id = msg->header.frame_id;
  Marker_msg_.header.stamp = ros::Time::now();
  Marker_msg_.ns = "basic_shapes";
  Marker_msg_.id = 0;
  Marker_msg_.type = shape;
  Marker_msg_.action = visualization_msgs::Marker::ADD;
  Marker_msg_.pose.position.x = centroid.x();
  Marker_msg_.pose.position.y = centroid.y();
  Marker_msg_.pose.position.z = centroid.z();
  Marker_msg_.pose.orientation.x = pose_c.x();
  Marker_msg_.pose.orientation.y = pose_c.y();
  Marker_msg_.pose.orientation.z = pose_c.z();
  Marker_msg_.pose.orientation.w = pose_c.w();
  Marker_msg_.scale.x = eigensolver.eigenvalues()[1]*10;
  Marker_msg_.scale.y = eigensolver.eigenvalues()[2]*10;
  Marker_msg_.scale.z = eigensolver.eigenvalues()[0]*10;
  Marker_msg_.color.r = 0.0f;
  Marker_msg_.color.g = 1.0f;
  Marker_msg_.color.b = 0.0f;
  Marker_msg_.color.a = 1.0;
  Marker_msg_.lifetime = ros::Duration();

  // Extract Poses
  geometry_msgs::PoseArray leaf_pose_array;
  leaf_pose_array.poses.clear();
  leaf_pose_array.header.stamp = ros::Time::now(); 
  leaf_pose_array.header.frame_id = msg->header.frame_id; 
  for (pcl::PointCloud<pcl::PointXYZ>::iterator it = cloud_cluster_right->begin(); it != cloud_cluster_right->end(); ++it){
    
    Eigen::Vector3f vx (it->x-centroid.x(), it->y-centroid.y(), it->z-centroid.z()); 
    Eigen::Vector3f vy (-eigensolver.eigenvectors()(0,2),-eigensolver.eigenvectors()(1,2),  -eigensolver.eigenvectors()(2,2)); 
    Eigen::Vector3f vz (-eigensolver.eigenvectors()(0,0),-eigensolver.eigenvectors()(1,0),  -eigensolver.eigenvectors()(2,0)); 
    vx = vx/vx.norm();
    vz = vx.cross(vy);
    vz = vz/vz.norm();
    vy = vz.cross(vx);
    vy = vy/vy.norm();

    Eigen::Matrix3f rot_mm;
    rot_mm << vx.x(), vy.x(), vz.x(),
              vx.y(), vy.y(), vz.y(),
              vx.z(), vy.z(), vz.z();
    Eigen::Quaternionf pose_cc(rot_mm);
    
    geometry_msgs::Pose pose;
    pose.position.x = it->x;
    pose.position.y = it->y;
    pose.position.z = it->z;
    pose.orientation.x = pose_cc.x();
    pose.orientation.y = pose_cc.y();
    pose.orientation.z = pose_cc.z();
    pose.orientation.w = pose_cc.w();
    leaf_pose_array.poses.push_back(pose);
  }
  std::sort(leaf_pose_array.poses.begin(), leaf_pose_array.poses.end(), MyDataSortPredicate);

  // initial intermediate pose
  geometry_msgs::Pose initial_pose;
  initial_pose = leaf_pose_array.poses.front();
  Eigen::Vector3f initial_point;
  initial_point.x() = initial_pose.position.x - centroid.x(); 
  initial_point.y() = initial_pose.position.y - centroid.y(); 
  initial_point.z() = initial_pose.position.z - centroid.z(); 
  initial_point.x() = initial_point.x()*(initial_point.norm()+0.1)/initial_point.norm(); 
  initial_point.y() = initial_point.y()*(initial_point.norm()+0.1)/initial_point.norm(); 
  initial_point.z() = initial_point.z()*(initial_point.norm()+0.1)/initial_point.norm(); 
  initial_pose.position.x =  initial_point.x() + centroid.x();
  initial_pose.position.y =  initial_point.y() + centroid.y();
  initial_pose.position.z =  initial_point.z() + centroid.z();
 
  // final intermediate pose
  geometry_msgs::Pose final_pose;
  final_pose = leaf_pose_array.poses.back();
  Eigen::Vector3f final_point;
  final_point.x() = final_pose.position.x - centroid.x(); 
  final_point.y() = final_pose.position.y - centroid.y(); 
  final_point.z() = final_pose.position.z - centroid.z(); 
  final_point.x() = final_point.x()*(final_point.norm()+0.1)/final_point.norm(); 
  final_point.y() = final_point.y()*(final_point.norm()+0.1)/final_point.norm(); 
  final_point.z() = final_point.z()*(final_point.norm()+0.1)/final_point.norm(); 
  final_pose.position.x =  final_point.x()+centroid.x();
  final_pose.position.y =  final_point.y()+centroid.y();
  final_pose.position.z =  final_point.z()+centroid.z();
 
  // add initial and final poses
  pose_array_.poses.clear();
  pose_array_.header.stamp = ros::Time::now(); 
  pose_array_.header.frame_id = msg->header.frame_id; 
  pose_array_.poses.push_back(initial_pose);
  for (std::vector<geometry_msgs::Pose>::iterator it = leaf_pose_array.poses.begin(); it < leaf_pose_array.poses.end(); ++it){
    pose_array_.poses.push_back(*it);
  }
  pose_array_.poses.push_back(final_pose);

//  // Estimate point normals
//  pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>); 
//  //pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
//  pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZ> ());
//  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
//  ne.setSearchMethod (tree);
//  ne.setInputCloud (cloud);
//  ne.setKSearch (50);
//  ne.compute (*normals);


  //unlock previously blocked shared variables 
  //alg_.unlock(); 
  //input_mutex_.exit(); 

  // publishing msgs
  output_marker_publisher_.publish(Marker_msg_);
  output_pointcloud_publisher_.publish(*cloud_cluster_right);
  poses_publisher_.publish(pose_array_);
  data_processed_ = true;
}

/*  [service callbacks] */
bool ZyonzFindLeafProbingPointsAlgNode::get_pose_arrayCallback(zyonz_msgs::GetPoseArray::Request &req, zyonz_msgs::GetPoseArray::Response &res) 
{ 
  ROS_INFO("ZyonzFindLeafProbingPointsAlgNode::get_pose_arrayCallback: New Request Received!"); 

  //use appropiate mutex to shared variables if necessary 
  //this->alg_.lock(); 
  //this->get_pose_array_mutex_.enter(); 

  if(data_processed_) 
  { 
    ROS_INFO("ZyonzFindLeafProbingPointsAlgNode::get_pose_arrayCallback: Processing New Request!"); 
    //do operations with req and output on res 
    //res.data2 = req.data1 + my_var; 
//    tf::StampedTransform current_transform;
//    obtain_current_pose (current_transform);
//    geometry_msgs::Pose current_pose;
//    current_pose.position.x = current_transform.getOrigin().x();
//    current_pose.position.y = current_transform.getOrigin().y();
//    current_pose.position.z = current_transform.getOrigin().z();
//    current_pose.orientation.x = current_transform.getRotation().x();
//    current_pose.orientation.y = current_transform.getRotation().y();
//    current_pose.orientation.z = current_transform.getRotation().z();
//    current_pose.orientation.w = current_transform.getRotation().w();
//    pose_array_.poses.push_back(current_pose);
    res.data = pose_array_;
  
  } 
  else 
  { 
    ROS_INFO("ZyonzFindLeafProbingPointsAlgNode::get_pose_arrayCallback: ERROR: alg has no data to request yet."); 
  } 

  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  //this->get_pose_array_mutex_.exit(); 

  return true; 
}

/*  [action callbacks] */

/*  [action requests] */
void ZyonzFindLeafProbingPointsAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();

  this->alg_.unlock();
}

void ZyonzFindLeafProbingPointsAlgNode::addNodeDiagnostics(void)
{
}

/* main function */
int main(int argc,char *argv[])
{
  return algorithm_base::main<ZyonzFindLeafProbingPointsAlgNode>(argc, argv, "zyonz_find_leaf_probing_points_alg_node");
}