BayesVisualization.h

Go to the documentation of this file.

#include <vector>

#include <ros/ros.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>

#include <tf/transform_listener.h>

#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

namespace Visualization {
  static const int ID_GAUSSIAN = 10;

  class BayesVisualization {
  private:
    ros::Publisher gaussianPublisher;
    ros::Publisher samplePublisher;
    ros::Publisher eigenPublisher;
                
  public:
    BayesVisualization();
    ~BayesVisualization();

    // visualizes a point cloud
    void drawSamples(std::vector<Eigen::Vector3d> samples);

    // draws a 3D gaussian distribution with given mean and covariance
    void drawDistribution(Eigen::Vector3d mean, Eigen::Matrix3d covariance);

    // draws a 3D gaussian distribution over the given points
    void drawDistribution(std::vector<Eigen::Vector3d> samples);

    // draws an arrow between two points
    visualization_msgs::Marker createArrowMarker(const Eigen::Vector3d a, const Eigen::Vector3d b);

    // creates a geometry message for the given point
    geometry_msgs::Point createPointMessage(Eigen::Vector3d point);
  };
}