covariance_markers_alg.cpp

Go to the documentation of this file.

#include "covariance_markers_alg.h"

CovarianceMarkersAlgorithm::CovarianceMarkersAlgorithm():
frame_id_("/teo/odom"),
namespace_("/teo"),
counter_(0),
scale_multiplier_(2.0),
scale_multiplier_z_(0.01),
color_alpha_(0.5),
color_red_(0.45),
color_green_(0.95),
color_blue_ (0.65)
{
}

CovarianceMarkersAlgorithm::~CovarianceMarkersAlgorithm()
{
}

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

  // save the current configuration
  config_ = new_cfg;
  
  frame_id_  = config_.frame_id;
  namespace_ = config_.ns;
  scale_multiplier_   = config_.scale_multiplier;
  scale_multiplier_z_ = config_.scale_multiplier_z;
  color_alpha_ = config_.color_alpha;
  color_red_   = config_.color_red;
  color_green_ = config_.color_green;
  color_blue_  = config_.color_blue;

  this->unlock();
}

// CovarianceMarkersAlgorithm Public API
void CovarianceMarkersAlgorithm::drawCovariance()
{
  current_marker_.header.stamp = ros::Time::now();
  current_marker_.header.frame_id = frame_id_;
  current_marker_.header.seq = counter_++;
  current_marker_.ns = namespace_;
  current_marker_.id = counter_;
  current_marker_.type = visualization_msgs::Marker::SPHERE;
  current_marker_.action = visualization_msgs::Marker::ADD;

  current_marker_.pose.position.x = odom_.pose.pose.position.x;
  current_marker_.pose.position.y = odom_.pose.pose.position.y;
  current_marker_.pose.position.z = odom_.pose.pose.position.z;

  ROS_DEBUG("CovarianceMarkersAlgorithm::drawCovariance x: %f y: %f z: %f",
                                                    odom_.pose.pose.position.x,
                                                    odom_.pose.pose.position.y,
                                                    odom_.pose.pose.position.z);

  Eigen::Matrix3f covs;
  covs(0) = odom_.pose.covariance[0]; // xx
  covs(1) = odom_.pose.covariance[1]; // xy
  covs(2) = odom_.pose.covariance[2]; // xz
  covs(3) = odom_.pose.covariance[6]; // yy
  covs(4) = odom_.pose.covariance[7]; // yx
  covs(5) = odom_.pose.covariance[8]; // yz
  covs(6) = odom_.pose.covariance[12]; // zx
  covs(7) = odom_.pose.covariance[13]; // zy
  covs(8) = odom_.pose.covariance[14]; // zz

  ROS_DEBUG("CovarianceMarkersAlgorithm::drawCovariance cov: %f %f %f %f",
                                              covs(0),covs(1),covs(2),covs(3));

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eig(covs);

  const Eigen::Vector3f& eigValues  (eig.eigenvalues());
  const Eigen::Matrix3f& eigVectors (eig.eigenvectors());

  // de moment nomes considerem rotacio en z
  float angle = (atan2(eigVectors(1, 0), eigVectors(0, 0)));

  double length_x = sqrt(eigValues[0]);
  double length_y = sqrt(eigValues[1]);
  double length_z = sqrt(eigValues[2]);

  ROS_DEBUG("CovarianceMarkersAlgorithm::drawCovariance lengths: %f %f %f",
                                                   length_x,length_y,length_z);

  current_marker_.scale.x = length_x*scale_multiplier_;
  current_marker_.scale.y = length_y*scale_multiplier_;
  current_marker_.scale.z = length_z*scale_multiplier_z_;

  current_marker_.color.a = color_alpha_;
  current_marker_.color.r = color_red_;
  current_marker_.color.g = color_green_;
  current_marker_.color.b = color_blue_;

  current_marker_.pose.orientation.w = cos(angle*0.5);
  current_marker_.pose.orientation.z = sin(angle*0.5);

  marker_array_.markers.push_back(current_marker_);
}

visualization_msgs::MarkerArray CovarianceMarkersAlgorithm::marker_array()
{
  return marker_array_;
}

// http://geus.wordpress.com/2011/09/15/how-to-represent-a-3d-normal-function-with-ros-rviz/


  // #getting the distribution eigen vectors and values
  // (eigValues,eigVectors) = numpy.linalg.eig (covMat)

  // #painting the eigen vectors
  // id=1
  // for v in eigVectors:
  //   m=markerVector(id, v*eigValues[id-1], mean)
  //   id=id+1
  //   points_pub.publish(m)

  // #building the rotation matrix
  // eigx_n=PyKDL.Vector(eigVectors[0,0],eigVectors[0,1],eigVectors[0,2])
  // eigy_n=-PyKDL.Vector(eigVectors[1,0],eigVectors[1,1],eigVectors[1,2])
  // eigz_n=PyKDL.Vector(eigVectors[2,0],eigVectors[2,1],eigVectors[2,2])
  // eigx_n.Normalize()
  // eigy_n.Normalize()
  // eigz_n.Normalize()
  //   rot = PyKDL.Rotation (eigx_n,eigy_n,eigz_n)
  // quat = rot.GetQuaternion ()



// https://code.ros.org/trac/ros-pkg/ticket/4857


  // void drawCovariance(const Eigen::Vector2f& mean, const Eigen::Matrix2f& covMatrix)
  //   {
  //     tempMarker.pose.position.x = mean[0];
  //     tempMarker.pose.position.y = mean[1];

  //     Eigen::SelfAdjointEigenSolver<Eigen::Matrix2f> eig(covMatrix);

  //     const Eigen::Vector2f& eigValues (eig.eigenvalues());
  //     const Eigen::Matrix2f& eigVectors (eig.eigenvectors());

  //     float angle = (atan2(eigVectors(1, 0), eigVectors(0, 0)));

  //     tempMarker.type = visualization_msgs::Marker::CYLINDER;

  //     double lengthMajor = sqrt(eigValues[0]);
  //     double lengthMinor = sqrt(eigValues[1]);

  //     tempMarker.scale.x = lengthMajor;
  //     tempMarker.scale.y = lengthMinor;
  //     tempMarker.scale.z = 0.001;

  //     tempMarker.pose.orientation.w = cos(angle*0.5);
  //     tempMarker.pose.orientation.z = sin(angle*0.5);

  //     tempMarker.id = idCounter++;
  //     markerArray.markers.push_back(tempMarker);
  //   }