zyonz_obtain_roi_jump_edge_based_alg.cpp

Go to the documentation of this file.

#include "zyonz_obtain_roi_jump_edge_based_alg.h"

ZyonzObtainRoiJumpEdgeBasedAlgorithm::ZyonzObtainRoiJumpEdgeBasedAlgorithm(void):
roi_pose_ptr_(boost::make_shared<geometry_msgs::PoseStamped>()),
viewpoint_pose_ptr_(boost::make_shared<geometry_msgs::PoseStamped>())
{
}

ZyonzObtainRoiJumpEdgeBasedAlgorithm::~ZyonzObtainRoiJumpEdgeBasedAlgorithm(void)
{
}

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

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

// ZyonzObtainRoiJumpEdgeBasedAlgorithm Public API
bool ZyonzObtainRoiJumpEdgeBasedAlgorithm::principal_component_analysis(pcl::PointCloud<pcl::PointXYZ>::ConstPtr point_cloud, Eigen::Vector4f& centroid, Eigen::Quaternionf& orientation)
{

  if (point_cloud->size()< 10)
    return false;

  // Compute the cluster centroid
  pcl::compute3DCentroid (*point_cloud, centroid);
  ROS_DEBUG("Cluster Centroid: (%f, %f, %f)", centroid(0), centroid(1), centroid(2));
  
  // Principal Component Analysis (PCA)
  // 1) Compute Covariance
  Eigen::MatrixXf B(point_cloud->points.size(),3);
  int aa = 0;
  for (pcl::PointCloud<pcl::PointXYZ>::const_iterator it = point_cloud->begin(); it != point_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);
  }
  
  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();
  orientation = Eigen::Quaternionf (rot_m);
  ROS_DEBUG("Quaternion(x, y, z, w): (%f, %f, %f, %f)", orientation.x(), orientation.y(), orientation.z(), orientation.w());

  return true;

}

bool ZyonzObtainRoiJumpEdgeBasedAlgorithm::isRunning(sensor_msgs::PointCloud2::ConstPtr& point_cloud_ConstPtr, sensor_msgs::Image::ConstPtr& cluster_a_img_ConstPtr, sensor_msgs::Image::ConstPtr& cluster_b_img_ConstPtr, sensor_msgs::Image::ConstPtr& residual_img_ConstPtr)
{
    // Conversion of ROSPointCloud2 to pcl
    pcl::fromROSMsg(*point_cloud_ConstPtr, roi_pc_);
    
    // Conversion of ROSImages to opencv
    //  - So we can play with them
    cv_bridge::CvImageConstPtr cluster_a_cv_ptr, cluster_b_cv_ptr, residual_cv_ptr;
    cv_bridge::CvImagePtr roi_cv_ptr;
    try
    {
        cluster_a_cv_ptr = cv_bridge::toCvShare(cluster_a_img_ConstPtr, sensor_msgs::image_encodings::MONO8);
        cluster_b_cv_ptr = cv_bridge::toCvShare(cluster_b_img_ConstPtr, sensor_msgs::image_encodings::MONO8);
        residual_cv_ptr = cv_bridge::toCvShare(residual_img_ConstPtr, sensor_msgs::image_encodings::MONO8);
        roi_cv_ptr = cv_bridge::toCvCopy(residual_img_ConstPtr, sensor_msgs::image_encodings::MONO8);
    }
    catch (cv_bridge::Exception& e)
    {  
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return false;
    }
    
    cv::Mat a_dilated_mat;
    cv::Mat b_dilated_mat;
    //cv::Mat element(7,7,CV_8U,cv::Scalar(1));
    cv::Mat element(3,3,CV_8U,cv::Scalar(1));

    // Dilate both clusters   
    cv::dilate(cluster_a_cv_ptr->image, a_dilated_mat, element, cv::Point(-1,-1), 3);
    cv::dilate(cluster_b_cv_ptr->image, b_dilated_mat, element, cv::Point(-1,-1), 3);
   
    //cv::dilate(cluster_a_cv_ptr->image, a_dilated_mat, element);
    //cv::dilate(cluster_b_cv_ptr->image, b_dilated_mat, element);
   
    // Logical AND of both dilated clusters 
    //  - So we get the ROI image
    cv::Mat temp_roi_mat;
    cv::bitwise_and(a_dilated_mat, b_dilated_mat, temp_roi_mat);
    cv::bitwise_and(temp_roi_mat, residual_cv_ptr->image, roi_cv_ptr->image);
    
    //obtain the roi_pc
    for (int rr = 0; rr < roi_cv_ptr->image.rows; ++rr)
        for (int cc = 0; cc < roi_cv_ptr->image.cols; ++cc)
            if (roi_cv_ptr->image.at<uchar>(rr,cc) != 255){
                roi_pc_(cc,rr).z = std::numeric_limits<float>::quiet_NaN ();
                //std::cout << roi_pc_.at(rr,cc).z << std::endl;
            }
    roi_img_ptr_ = roi_cv_ptr->toImageMsg();

    /* 
     * COMPUTING THE ROI POSE
     */
    // Compute the cluster centroid
    if (roi_pc_.size() < 10)
    {
        ROS_INFO("ZyonzObtainRoiJumpEdgeBasedAlgorithm::isRunning: ERROR: Not enough supporting points in the centroid computation");
        return false;
    }
    Eigen::Vector4f centroid;
    pcl::compute3DCentroid (roi_pc_, centroid);

    roi_pose_ptr_->header = point_cloud_ConstPtr->header;
    roi_pose_ptr_->pose.position.x = centroid.x();
    roi_pose_ptr_->pose.position.y = centroid.y();
    roi_pose_ptr_->pose.position.z = centroid.z();
    // The orientation is the one of the camera
    //   - Why? Because by definition the plane should pass through the camera
    roi_pose_ptr_->pose.orientation.x = 0;
    roi_pose_ptr_->pose.orientation.y = 0;
    roi_pose_ptr_->pose.orientation.z = 0;
    roi_pose_ptr_->pose.orientation.w = 1;
    ROS_DEBUG("Cluster Centroid: (%f, %f, %f)", roi_pose_ptr_->pose.position.x, roi_pose_ptr_->pose.position.y, roi_pose_ptr_->pose.position.z);
    ROS_DEBUG("Quaternion(x, y, z, w): (%f, %f, %f, %f)", roi_pose_ptr_->pose.orientation.x, roi_pose_ptr_->pose.orientation.y, roi_pose_ptr_->pose.orientation.z, roi_pose_ptr_->pose.orientation.w);


    // Reading the Pose of the pointcloud sensor
    tf::TransformListener tf_listener; 
    tf::StampedTransform transform;
    try
    {
      tf_listener.waitForTransform(std::string("wam_link0"), roi_pc_.header.frame_id, ros::Time::now(), ros::Duration(1.0));
      tf_listener.lookupTransform(std::string("wam_link0"), roi_pc_.header.frame_id, ros::Time(0), transform);
    }
    catch (tf::TransformException ex)
    {
      ROS_ERROR("%s", ex.what ());
    }
    tf::poseTFToMsg(transform, viewpoint_pose_ptr_->pose);
    viewpoint_pose_ptr_->header.stamp = transform.stamp_;
    viewpoint_pose_ptr_->header.frame_id = transform.frame_id_;

    return true;
} 

pcl::PointCloud<pcl::PointXYZ>::ConstPtr ZyonzObtainRoiJumpEdgeBasedAlgorithm::get_roi_pc(void)
{
    return roi_pc_.makeShared();
}

sensor_msgs::Image::Ptr ZyonzObtainRoiJumpEdgeBasedAlgorithm::get_roi_img(void)
{
    return roi_img_ptr_;
}

geometry_msgs::PoseStamped::Ptr ZyonzObtainRoiJumpEdgeBasedAlgorithm::get_roi_pose(void)
{
    return roi_pose_ptr_;
}

geometry_msgs::PoseStamped::Ptr ZyonzObtainRoiJumpEdgeBasedAlgorithm::get_viewpoint_pose(void)
{
    return viewpoint_pose_ptr_;
}