zyonz_image_based_leaf_probing_alg.cpp

Go to the documentation of this file.

#include "zyonz_image_based_leaf_probing_alg.h"
#include "segment-image.h"
#include "leaf-fitting.h"
#include "leaf-parameters.h"
#include "find-approach.h"
#include "pnmfile.h"
// [opencv2]                          
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <sensor_msgs/image_encodings.h>

ZyonzImageBasedLeafProbingAlgorithm::ZyonzImageBasedLeafProbingAlgorithm(void)
{
  probing_point_ = Eigen::Vector3f(0.0, 0.0, 0.0);
}

ZyonzImageBasedLeafProbingAlgorithm::~ZyonzImageBasedLeafProbingAlgorithm(void)
{
}

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

  ROS_INFO("Reconfigure request: %f, level %d", new_cfg.sigma, level);
  ROS_INFO("Reconfigure request: %f, level %d", new_cfg.k, level);
  ROS_INFO("Reconfigure request: %d, level %d", new_cfg.min_size, level);

  set_sigma(new_cfg.sigma);
  set_k(new_cfg.k);
  set_min_size(new_cfg.min_size);
  
  // save the current configuration
  this->config_=new_cfg;
  
  this->unlock();
}

// ZyonzImageBasedLeafProbingAlgorithm Public API

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

  // Convert from msg to algorithm based inputs
  pcl::fromROSMsg(*msg, pcl_xyzrgb_);
  image<rgb> *input;
  image<rgb> *point_cloud;
  input = new image<rgb>(pcl_xyzrgb_.height, pcl_xyzrgb_.width);
  point_cloud = new image<rgb>(pcl_xyzrgb_.height, pcl_xyzrgb_.width);
  pcl::PointCloud<pcl::PointXYZRGB>::iterator pcl_iter = pcl_xyzrgb_.begin();
  for (int rr = 0; rr < pcl_xyzrgb_.height; ++rr) {
    for (int cc = 0; cc < pcl_xyzrgb_.width; ++cc, ++pcl_iter) {
      input->access[rr][cc].r = pcl_iter->r;
      input->access[rr][cc].g = pcl_iter->g;
      input->access[rr][cc].b = pcl_iter->b;
      point_cloud->access[rr][cc].r = pcl_iter->x*100+100;
      point_cloud->access[rr][cc].g = pcl_iter->y*100+100;
      point_cloud->access[rr][cc].b = pcl_iter->z*100;
    }
  }

  // SEGMENTATION
  
  float sigma = get_sigma();
  float k = get_k();
  int min_size = get_min_size();
  //outputs
  int ** cluster_labels;
  int num_ccs;
  int num_ccs_fin;

  image<rgb> *seg = segment_image(input, point_cloud, sigma, k, min_size, &num_ccs, &num_ccs_fin, cluster_labels);
  //ROS_INFO("image size: %d, %d ", seg->height(), seg->width());
  cv::Mat seg_image(seg->height(), seg->width(), CV_8UC3);
  cv::Mat_<cv::Vec3b>::iterator img_iter = seg_image.begin<cv::Vec3b>();
  for (int rr = 0; rr < seg_image.rows; ++rr) {
    for (int cc = 0; cc < seg_image.cols; ++cc, ++img_iter ) {
      (*img_iter)[0] = seg->access[rr][cc].r;
      (*img_iter)[1] = seg->access[rr][cc].g;
      (*img_iter)[2] = seg->access[rr][cc].b;
    }
  }
  seg_cv_ptr_ = boost::make_shared<cv_bridge::CvImage>();
  seg_cv_ptr_->header = msg->header;
  seg_cv_ptr_->encoding = sensor_msgs::image_encodings::RGB8;
  seg_cv_ptr_->image = seg_image;
  

  // LEAF CONFIDENCE

//  image<rgb> *leaf_model = loadPPM("/home/sfoix/programming/iri/iri-ros-pkg/zyonz_robot/zyonz_image_based_leaf_probing/model/model_F.ppm");
//  image<rgb> *leaf_model_area = loadPPM("/home/sfoix/programming/iri/iri-ros-pkg/zyonz_robot/zyonz_image_based_leaf_probing/model/model_F_bound.ppm");
//  int model_probing_point [2] = {107, 93}; // Calathea Compactstar 

//  image<rgb> *leaf_model = loadPPM("/home/sfoix/programming/iri/iri-ros-pkg/zyonz_robot/zyonz_image_based_leaf_probing/model/model_E.ppm");
//  image<rgb> *leaf_model_area = loadPPM("/home/sfoix/programming/iri/iri-ros-pkg/zyonz_robot/zyonz_image_based_leaf_probing/model/model_E_bound.ppm");
//  int model_probing_point [2] = {110, 100}; // Anthurium_Andreanum (WHITE)

//  image<rgb> *leaf_model = loadPPM("/home/sfoix/programming/iri/iri-ros-pkg/zyonz_robot/zyonz_image_based_leaf_probing/model/model_A.ppm");
//  image<rgb> *leaf_model_area = loadPPM("/home/sfoix/programming/iri/iri-ros-pkg/zyonz_robot/zyonz_image_based_leaf_probing/model/model_A_bound.ppm");
//  int model_probing_point [2] = {138, 108}; // Anthurium_Andreanum (RED)

//  image<rgb> *leaf_model = loadPPM("/home/sfoix/programming/iri/iri-ros-pkg/zyonz_robot/zyonz_image_based_leaf_probing/model/model_T.ppm");
//  image<rgb> *leaf_model_area = loadPPM("/home/sfoix/programming/iri/iri-ros-pkg/zyonz_robot/zyonz_image_based_leaf_probing/model/model_T_bound.ppm");
//  int model_probing_point [2] = {86, 110}; // Tobacco 
  
  image<rgb> *leaf_model = loadPPM("/home/sfoix/programming/iri/iri-ros-pkg/zyonz_robot/zyonz_image_based_leaf_probing/model/model_P.ppm");
  image<rgb> *leaf_model_area = loadPPM("/home/sfoix/programming/iri/iri-ros-pkg/zyonz_robot/zyonz_image_based_leaf_probing/model/model_P_bound.ppm");
  int model_probing_point [2] = {107, 103}; // Pothos 
  
  double *fit_score_list;
  double **transformation_parameters;
  image<rgb> *leaf_confidence = leaf_fitting(cluster_labels, leaf_model, leaf_model_area, num_ccs_fin, fit_score_list, transformation_parameters);
  //ROS_INFO("transformation_paramters[0][0]: %f", transformation_parameters[0][0]);
  cv::Mat confidence_image(leaf_confidence->height(), leaf_confidence->width(), CV_8UC3);
  cv::Mat_<cv::Vec3b>::iterator conf_img_iter = confidence_image.begin<cv::Vec3b>();
  for (int rr = 0; rr < confidence_image.rows; ++rr) {
    for (int cc = 0; cc < confidence_image.cols; ++cc, ++conf_img_iter ) {
      (*conf_img_iter)[0] = leaf_confidence->access[rr][cc].r;
      (*conf_img_iter)[1] = leaf_confidence->access[rr][cc].g;
      (*conf_img_iter)[2] = leaf_confidence->access[rr][cc].b;
    }
  }
  confidence_cv_ptr_ = boost::make_shared<cv_bridge::CvImage>();
  confidence_cv_ptr_->header = msg->header;
  confidence_cv_ptr_->encoding = sensor_msgs::image_encodings::RGB8;
  confidence_cv_ptr_->image = confidence_image;

  // LEAF PARAMETERS
  goal_parameters_ = leaf_parameters(cluster_labels, point_cloud, num_ccs_fin, fit_score_list);
  ROS_INFO("goal_parameters %f x_c ", goal_parameters_[0]-100);
  ROS_INFO("goal_parameters %f y_c ", goal_parameters_[1]-100);    
  ROS_INFO("goal_parameters %f z_c ", goal_parameters_[2]);
  ROS_INFO("goal_parameters %f n_x ", goal_parameters_[3]);
  ROS_INFO("goal_parameters %f n_y ", goal_parameters_[4]);
  ROS_INFO("goal_parameters %f n_z ", goal_parameters_[5]);
  ROS_INFO("goal_parameters %f label ", goal_parameters_[6]);
  // Leaf centroid in [m]
  Eigen::Vector3d leaf_centroid(goal_parameters_[0]-100, goal_parameters_[1]-100, goal_parameters_[2]);   


  // PROBING PARAMETERS
  double* probing_movement_parameters = find_approach(cluster_labels, point_cloud, goal_parameters_, transformation_parameters, model_probing_point); 
 
  // Probing point in [m]
  probing_movement_parameters[3] = (probing_movement_parameters[3]-100)/100;
  probing_movement_parameters[4] = (probing_movement_parameters[4]-100)/100;
  probing_movement_parameters[5] = (probing_movement_parameters[5])/100;

  probing_point_ = Eigen::Vector3f(probing_movement_parameters[3], probing_movement_parameters[4], probing_movement_parameters[5]); 
  Eigen::Vector3f x_vector(probing_movement_parameters[0], probing_movement_parameters[1], probing_movement_parameters[2]); 
  Eigen::Vector3f normal_vector(goal_parameters_[3], goal_parameters_[4], goal_parameters_[5]); 

  Eigen::Quaternionf probing_pose = Eigen::Quaternionf(1.0, 0.0, 0.0, 0.0);
  
  find_probing_pose_by_PCA( goal_parameters_[6], cluster_labels, point_cloud, probing_pose, normal_vector);

  find_probing_pose( probing_point_, x_vector, normal_vector, probing_pose);

  //Filling in the probing step msg
  //
  // probing Point
  geometry_msgs::PoseStamped temp_pose_st;
  temp_pose_st.header = msg->header; 
  //ROS_INFO("FRAME_ID: %i", temp_pose_st.header.frame_id);
  temp_pose_st.header.stamp = ros::Time::now(); 
  temp_pose_st.pose.position.x = probing_point_.x(); 
  temp_pose_st.pose.position.y = probing_point_.y(); 
  temp_pose_st.pose.position.z = probing_point_.z(); 
  temp_pose_st.pose.orientation.x = probing_pose.x()/probing_pose.norm(); 
  temp_pose_st.pose.orientation.y = probing_pose.y()/probing_pose.norm(); 
  temp_pose_st.pose.orientation.z = probing_pose.z()/probing_pose.norm(); 
  temp_pose_st.pose.orientation.w = probing_pose.w()/probing_pose.norm(); 
  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 = probing_point_.x() + x_vector.x()*(x_vector.norm()-0.90)/x_vector.norm();
  temp_pose_st.pose.position.y = probing_point_.y() + x_vector.y()*(x_vector.norm()-0.90)/x_vector.norm();
  temp_pose_st.pose.position.z = probing_point_.z() + x_vector.z()*(x_vector.norm()-0.90)/x_vector.norm();
  temp_pose_st.pose.orientation.x = probing_pose.x()/probing_pose.norm();
  temp_pose_st.pose.orientation.y = probing_pose.y()/probing_pose.norm();
  temp_pose_st.pose.orientation.z = probing_pose.z()/probing_pose.norm();
  temp_pose_st.pose.orientation.w = probing_pose.w()/probing_pose.norm();
  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;

  // Find grasp point and approach vector
  ROS_INFO("probing point(x, y, z): (%f, %f, %f)",  probing_step_.probing.pose.position.x, probing_step_.probing.pose.position.y, probing_step_.probing.pose.position.z);
  ROS_INFO("approaching pre point(x, y, z): (%f, %f, %f)",  probing_step_.pre_probing.poses[0].position.x, probing_step_.pre_probing.poses[0].position.y, probing_step_.pre_probing.poses[0].position.z);
  ROS_INFO("approaching post point(x, y, z): (%f, %f, %f)",  probing_step_.post_probing.poses[0].position.x, probing_step_.post_probing.poses[0].position.y, probing_step_.post_probing.poses[0].position.z);
  ROS_INFO("probing x_vector(x, y, z): (%f, %f, %f)",  x_vector.x(), x_vector.y(), x_vector.z());
  ROS_INFO("pose quaternion(x, y, z, w): (%f, %f, %f, %f)",  probing_pose.x(), probing_pose.y(), probing_pose.z(), probing_pose.w());

  return true;
}

void ZyonzImageBasedLeafProbingAlgorithm::find_probing_pose(const Eigen::Vector3f probing_point, const Eigen::Vector3f x_vector, const Eigen::Vector3f normal_vector, Eigen::Quaternionf &probing_pose){
 
  Eigen::Vector3f y_vector;
  Eigen::Vector3f z_vector;
  y_vector = normal_vector.cross(x_vector);
  z_vector = x_vector.cross(y_vector);
  
  // Checking the direction of the Z axis respect the camera
  if (z_vector.z()<0){
    z_vector = -z_vector;
    y_vector = -y_vector;
  }

  Eigen::Matrix3f pose_matrix;
  pose_matrix << x_vector.x(), y_vector.x(), z_vector.x(),
                 x_vector.y(), y_vector.y(), z_vector.y(),
                 x_vector.z(), y_vector.z(), z_vector.z();
  //pose_matrix << x_vector.x(), x_vector.y(), x_vector.z(),
  //               y_vector.x(), y_vector.y(), y_vector.z(),
  //               z_vector.x(), z_vector.y(), z_vector.z();
  probing_pose = Eigen::Quaternionf(pose_matrix);
}


void ZyonzImageBasedLeafProbingAlgorithm::find_probing_pose_by_PCA(const int label, int ** clusters, const image<rgb> *pc, Eigen::Quaternionf &probing_pose, Eigen::Vector3f &normal_vector){

  // Principal Component Analysis (PCA)

  // number of points into the cluster?
  int num_points = 0;
  for (int cc = 0; cc < pc->width(); cc++)
  {
    for (int rr = 0; rr < pc->height(); rr++)
    {
      if (label == clusters[rr][cc])
      {
        num_points++;
      }
    }
  }
  
  ROS_INFO("(num_points, width, height): (%i, %d, %d)", num_points, pc->width(), pc->height());
  Eigen::MatrixXf B(num_points, 3);
  int row_idx = 0;
  for (int cc = 0; cc < pc->width(); cc++)
  {
    for (int rr = 0; rr < pc->height(); rr++)
    {
      if (clusters[rr][cc] == label)
      {
        B.row(row_idx)[0] = ((imRef(pc, cc, rr).r)-100.0)/100;
        B.row(row_idx)[1] = ((float)(imRef(pc, cc, rr).g)-100.0)/100;
        B.row(row_idx)[2] = ((float)(imRef(pc, cc, rr).b))/100;
        ROS_INFO("points(x, y, z): (%f, %f, %f)", B.row(row_idx)[0], B.row(row_idx)[1], B.row(row_idx)[2] );
        ++row_idx;
      }
    }
  }

  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 o_v(svd.matrixV().col(0)); //y
  Eigen::Vector3f n_v(svd.matrixV().col(1)); //x
  Eigen::Vector3f a_v(svd.matrixV().col(2)); //z
  // 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();
  probing_pose = Eigen::Quaternionf (rot_m);

  ROS_INFO("Normal(x, y, z): (%f, %f, %f)", a_v.x(), a_v.y(), a_v.z());
  ROS_INFO("Quaternion(x, y, z, w): (%f, %f, %f, %f)", probing_pose.x(), probing_pose.y(), probing_pose.z(), probing_pose.w());
  
  normal_vector = Eigen::Vector3f(o_v.x(), o_v.y(), o_v.z()); 
  
}


void ZyonzImageBasedLeafProbingAlgorithm::set_sigma(float new_sigma){
      sigma_ = new_sigma;
}

void ZyonzImageBasedLeafProbingAlgorithm::set_k(float new_k){
      k_ = new_k;
}

void ZyonzImageBasedLeafProbingAlgorithm::set_min_size(int new_min_size){
      min_size_ = new_min_size;
}

float ZyonzImageBasedLeafProbingAlgorithm::get_sigma(){
      return sigma_;
}

float ZyonzImageBasedLeafProbingAlgorithm::get_k(){
      return k_;
}

int ZyonzImageBasedLeafProbingAlgorithm::get_min_size(){
      return min_size_;
}

cv_bridge::CvImagePtr ZyonzImageBasedLeafProbingAlgorithm::get_seg_image(){
      return seg_cv_ptr_;
}

cv_bridge::CvImagePtr ZyonzImageBasedLeafProbingAlgorithm::get_confidence_image(){
      return confidence_cv_ptr_;
}

Eigen::Vector3f ZyonzImageBasedLeafProbingAlgorithm::get_probing_point(){

      return probing_point_;
}

zyonz_msgs::ProbingSteps ZyonzImageBasedLeafProbingAlgorithm::get_probing_step(){
      return probing_step_;
}