zyonz_geometric_based_single_leaf_probing_alg.cpp

Go to the documentation of this file.

#include "zyonz_geometric_based_single_leaf_probing_alg.h"
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/surface/convex_hull.h>
#include <pcl/surface/concave_hull.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/features/boundary.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/kdtree/kdtree_flann.h>

#include <tf/transform_broadcaster.h>
#include <tf/tfMessage.h>

#include <visualization_msgs/Marker.h>
#include <geometry_msgs/PoseArray.h>

ZyonzGeometricBasedSingleLeafProbingAlgorithm::ZyonzGeometricBasedSingleLeafProbingAlgorithm(void)
{
}

ZyonzGeometricBasedSingleLeafProbingAlgorithm::~ZyonzGeometricBasedSingleLeafProbingAlgorithm(void)
{
}

void ZyonzGeometricBasedSingleLeafProbingAlgorithm::config_update(Config& new_cfg, uint32_t level)
{
  this->lock();

  // save the current configuration
  this->config_=new_cfg;
  
  this->unlock();
}

// ZyonzGeometricBasedSingleLeafProbingAlgorithm Public API

bool ZyonzGeometricBasedSingleLeafProbingAlgorithm::get_leaf_probing_point(const sensor_msgs::PointCloud2::ConstPtr& msg)
{

  // Convert from msg to algorithm based inputs
  pcl::PointCloud<pcl::PointXYZ>::Ptr temp (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromROSMsg(*msg, *temp);

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  // This way the NaN values get removed  
  pcl::PassThrough<pcl::PointXYZ> pass;
  pass.setInputCloud (temp);
  pass.filter (*cloud);

  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
  pcl::compute3DCentroid (*cloud, leaf_centroid_);
  // ROS_INFO("Cluster Centroid: (%f, %f, %f)", leaf_centroid_(0), leaf_centroid_(1), leaf_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 - leaf_centroid_(0);
    B.row(aa)[1] = it->y - leaf_centroid_(1);
    B.row(aa)[2] = it->z - leaf_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;
  
  Eigen::JacobiSVD<Eigen::MatrixXf> svd(B, Eigen::ComputeThinU | Eigen::ComputeThinV);
  std::cout << "SingularValues are: \n" << svd.singularValues() << std::endl;
  //std::cout << "Left Singular Vectors are: \n" << svd.matrixU() << std::endl;
  std::cout << "Right Singular Vectors are: \n" << svd.matrixV() << std::endl;

  Eigen::Vector3f n_v(svd.matrixV().col(1));
  Eigen::Vector3f o_v(svd.matrixV().col(0));
  Eigen::Vector3f a_v(svd.matrixV().col(2));
  // Checking the direction of the Z axis respect the camera
  if (a_v.z()<0){
    a_v = -a_v;
    n_v = -n_v;
  }

  Eigen::Matrix3f rot_m;
  rot_m << n_v.x(), o_v.x(), a_v.x(),
           n_v.y(), o_v.y(), a_v.y(),
           n_v.z(), o_v.z(), a_v.z();
  leaf_pose_ = Eigen::Quaternionf (rot_m);
  ROS_INFO("Quaternion(x, y, z, w): (%f, %f, %f, %f)", leaf_pose_.x(), leaf_pose_.y(), leaf_pose_.z(), leaf_pose_.w());

  //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 = leaf_centroid_.x();
  Marker_msg_.pose.position.y = leaf_centroid_.y();
  Marker_msg_.pose.position.z = leaf_centroid_.z();
  Marker_msg_.pose.orientation.x = leaf_pose_.x();
  Marker_msg_.pose.orientation.y = leaf_pose_.y();
  Marker_msg_.pose.orientation.z = leaf_pose_.z();
  Marker_msg_.pose.orientation.w = leaf_pose_.w();
  Marker_msg_.scale.x = svd.singularValues()[1]/(svd.singularValues()[0]*10); // Scaled based on the biggest one
  Marker_msg_.scale.y = svd.singularValues()[0]/(svd.singularValues()[0]*10);
  Marker_msg_.scale.z = svd.singularValues()[2]/(svd.singularValues()[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::PoseStamped temp_pose_st;
  temp_pose_st.header.stamp = ros::Time::now();
  temp_pose_st.header.frame_id = msg->header.frame_id;
  temp_pose_st.pose.position.x = leaf_centroid_.x() + n_v.x()*(n_v.norm()-0.97)/n_v.norm();
  temp_pose_st.pose.position.y = leaf_centroid_.y() + n_v.y()*(n_v.norm()-0.97)/n_v.norm();
  temp_pose_st.pose.position.z = leaf_centroid_.z() + n_v.z()*(n_v.norm()-0.97)/n_v.norm();
  temp_pose_st.pose.orientation.x = leaf_pose_.x();
  temp_pose_st.pose.orientation.y = leaf_pose_.y();
  temp_pose_st.pose.orientation.z = leaf_pose_.z();
  temp_pose_st.pose.orientation.w = leaf_pose_.w();
  try{
      tf_listener_.waitForTransform(std::string("wam_link0"), temp_pose_st.header.frame_id, ros::Time::now(), ros::Duration(1.0));
      tf_listener_.transformPose(std::string("wam_link0"), temp_pose_st, probing_step_.probing);
  }catch (tf::TransformException ex){
      ROS_ERROR("%s", ex.what());
  }

  // pre probing point
  temp_pose_st.header = msg->header;
  temp_pose_st.header.stamp = ros::Time::now();
  temp_pose_st.pose.position.x = temp_pose_st.pose.position.x + n_v.x()*(n_v.norm()-0.90)/n_v.norm();
  temp_pose_st.pose.position.y = temp_pose_st.pose.position.y + n_v.y()*(n_v.norm()-0.90)/n_v.norm();
  temp_pose_st.pose.position.z = temp_pose_st.pose.position.z + n_v.z()*(n_v.norm()-0.90)/n_v.norm();
  temp_pose_st.pose.orientation.x = leaf_pose_.x();
  temp_pose_st.pose.orientation.y = leaf_pose_.y();
  temp_pose_st.pose.orientation.z = leaf_pose_.z();
  temp_pose_st.pose.orientation.w = leaf_pose_.w();
  geometry_msgs::PoseStamped pre_prob_st;
  try{
    tf_listener_.waitForTransform(std::string("wam_link0"), temp_pose_st.header.frame_id, ros::Time::now(), ros::Duration(1.0));
    tf_listener_.transformPose(std::string("wam_link0"), temp_pose_st, pre_prob_st);
  }catch (tf::TransformException ex){
    ROS_ERROR("%s", ex.what());
  }
  probing_step_.pre_probing.header = msg->header;
  probing_step_.pre_probing.header.stamp = ros::Time::now();
  probing_step_.pre_probing.header.frame_id = std::string("wam_link0");
  probing_step_.pre_probing.poses.clear();
  probing_step_.pre_probing.poses.push_back(pre_prob_st.pose);
  
  // post probing point
  probing_step_.post_probing = probing_step_.pre_probing;
  
  //unlock previously blocked shared variables 
  //alg_.unlock(); 
  //input_mutex_.exit(); 

  // publishing msgs

  return true;
}

visualization_msgs::Marker ZyonzGeometricBasedSingleLeafProbingAlgorithm::getMarker(){
  return Marker_msg_; 
}

Eigen::Vector4f ZyonzGeometricBasedSingleLeafProbingAlgorithm::getLeafCentroid(){
  return leaf_centroid_;
}

Eigen::Quaternionf ZyonzGeometricBasedSingleLeafProbingAlgorithm::getLeafPose(){
  return leaf_pose_;
}

zyonz_msgs::ProbingSteps ZyonzGeometricBasedSingleLeafProbingAlgorithm::getProbingSteps(){
  return probing_step_;
}