pointcloud_to_eigenvectors_alg.cpp

Go to the documentation of this file.

#include "pointcloud_to_eigenvectors_alg.h"

PointcloudToEigenvectorsAlgorithm::PointcloudToEigenvectorsAlgorithm(void)
{
}

PointcloudToEigenvectorsAlgorithm::~PointcloudToEigenvectorsAlgorithm(void)
{
}

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

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

// PointcloudToEigenvectorsAlgorithm Public API

bool PointcloudToEigenvectorsAlgorithm::get_eigenvectors(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
//TODO:

  // 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);

  // Compute the cluster centroid
  pcl::compute3DCentroid (*cloud, plane_centroid_);
  // ROS_INFO("Cluster Centroid: (%f, %f, %f)", plane_centroid_(0), plane_centroid_(1), plane_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 - plane_centroid_(0);
    B.row(aa)[1] = it->y - plane_centroid_(1);
    B.row(aa)[2] = it->z - plane_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();
  plane_pose_ = Eigen::Quaternionf (rot_m);
  ROS_INFO("Quaternion(x, y, z, w): (%f, %f, %f, %f)", plane_pose_.x(), plane_pose_.y(), plane_pose_.z(), plane_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 = plane_centroid_.x();
  Marker_msg_.pose.position.y = plane_centroid_.y();
  Marker_msg_.pose.position.z = plane_centroid_.z();
  Marker_msg_.pose.orientation.x = plane_pose_.x();
  Marker_msg_.pose.orientation.y = plane_pose_.y();
  Marker_msg_.pose.orientation.z = plane_pose_.z();
  Marker_msg_.pose.orientation.w = plane_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();


  return true;
}

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

Eigen::Vector4f PointcloudToEigenvectorsAlgorithm::getPlaneCentroid(){
  return plane_centroid_;
}

Eigen::Quaternionf PointcloudToEigenvectorsAlgorithm::getPlanePose(){
  return plane_pose_;
}