tools.cpp

Go to the documentation of this file.

#include "tools.h"
#include <iostream>
#include <pcl/filters/passthrough.h>
#include <Eigen/SVD>

#include <cmath>

geometry_msgs::PoseStamped getPoseStamped(const Eigen::Vector3f& pos, const tf::Quaternion& rotation){
  geometry_msgs::PoseStamped posestamped;
  posestamped.header.stamp = ros::Time::now();
  posestamped.header.frame_id = "/torso_lift_link" ;
  posestamped.pose.position.x = pos(0);
  posestamped.pose.position.y = pos(1);
  posestamped.pose.position.z = pos(2);
  posestamped.pose.orientation.w = rotation.getW();
  posestamped.pose.orientation.x = rotation.getX();
  posestamped.pose.orientation.y = rotation.getY();
  posestamped.pose.orientation.y = rotation.getY();
  return posestamped;
}

visualization_msgs::Marker visualize_filter_box(float xmin,float xmax,float ymin,float ymax,float zmin,float zmax, std_msgs::Header header){
  visualization_msgs::Marker marker;
  marker.header = header;
  //marker.header.stamp = ros::Time::now();
  marker.ns = "door_manipulation";
  marker.id = 0;
  marker.type = visualization_msgs::Marker::CUBE;
  marker.action = visualization_msgs::Marker::MODIFY;
  marker.pose.position.x = xmin+(fabs(xmin-xmax)/2.0);
  marker.pose.position.y = ymin+(fabs(ymin-ymax)/2.0);
  marker.pose.position.z = zmin+(fabs(zmin-zmax)/2.0);
  marker.pose.orientation.x = 0.0;
  marker.pose.orientation.y = 0.0;
  marker.pose.orientation.z = 0.0;
  marker.pose.orientation.w = 1.0;
  marker.scale.x = fabs((xmin-xmax));
  marker.scale.y = fabs((ymin-ymax));
  marker.scale.z = fabs((zmin-zmax));
  marker.color.a = 0.3;
  marker.color.r = 0.0;
  marker.color.g = 1.0;
  marker.color.b = 1.0;
  return marker;
}

bool computeMainAxis(const myCloudType& pc, Eigen::Vector3f& mean, Eigen::Vector3f& main_axis, float eval_ratio_threshold){
  using namespace Eigen;

  mean = Vector3f::Zero();
  for(unsigned int i = 0; i < pc.size(); i++){
    const myPointType& p = pc.points.at(i);
    mean += p.getVector3fMap ();
  }
  mean /= static_cast<float>(pc.size());

  Matrix3f cov = Matrix3f::Zero();
  for(unsigned int i = 0; i < pc.size(); i++){
    const myPointType& p = pc.points.at(i);
    Vector3f v = p.getVector3fMap() - mean;
    cov += v * v.adjoint();
  }
  cov /= static_cast<float>(pc.size());

  JacobiSVD<Matrix3f> svd(cov, ComputeFullU);
  Matrix3f eigenvectors = svd.matrixU();
  //use first col as main eigenvector
  main_axis = eigenvectors.col(0); 

  //std::cout << "EigenVectors:" << std::endl << eigenvectors << std::endl;
  //std::cout << "EigenValues:\n" << svd.singularValues() << std::endl;
  Vector3f evals = svd.singularValues();
  if(evals(0) / evals(1) < eval_ratio_threshold){
      ROS_WARN_THROTTLE(5, "Low Eigenvalue Ratio: %f", (evals(0) / evals(1)));
      return false;
  }else 
      return true;
}
    
void box_filter(myCloudType::Ptr pc, float xmin,float xmax,float ymin,float ymax,float zmin,float zmax){
  // Create the filtering object
  pcl::PassThrough<myPointType> pass;
  pass.setInputCloud (pc);

  pass.setFilterFieldName ("z");
  pass.setFilterLimits (zmin, zmax);
  pass.filter (*pc);

  pass.setFilterFieldName ("y");
  pass.setFilterLimits (ymin, ymax);
  pass.filter (*pc);

  pass.setFilterFieldName ("x");
  pass.setFilterLimits (xmin, xmax);
  pass.filter (*pc);

}