GaussianVisualization.h

Go to the documentation of this file.

# pragma once

// Global includes
#include <vector>

// Package includes
#include <ros/ros.h>
#include <std_msgs/ColorRGBA.h>
#include <visualization_msgs/Marker.h>

#include <tf/LinearMath/Vector3.h>
#include <tf/LinearMath/Matrix3x3.h>
#include <tf/LinearMath/Transform.h>
#include <tf/transform_datatypes.h>

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

namespace Visualization
{
  class GaussianVisualization
  {
  public:
    
    GaussianVisualization();
    
    ~GaussianVisualization();
    
    void setKernel(const boost::shared_ptr<Eigen::Vector3d>& pMean, const boost::shared_ptr<Eigen::Matrix3d>& pCovariance);
    
    void setScaleFactor(double pScale);
    
    void setObjectLocationFrameID(const std::string& pObjectLocationFrameID);
    
    void setAlphaChannel(const float pAlphaChannel);
    
    void setSigmaMultiplicator(const float pSigmaMultiplicator);
    
    void publishCovarianceEllipsoidMarker(const int pMarkerID, const std_msgs::ColorRGBA& pEllipsoidColor);
      
    boost::shared_ptr<visualization_msgs::Marker> generateCovarianceEllipsoidMarker(const int pMarkerID, const int pMarkerShape, const std::vector<std::vector<std::vector<float> > >& pWireFramePolygons, const float& pWireFrameWidth, const std_msgs::ColorRGBA& pEllipsoidColor);
    
    void publishCovarianceEllipsoidCoordinateSystemMarker(const int pMarkerID);
    
    void publishArrowToPoint(const int pMarkerID, const std_msgs::ColorRGBA& pColor, const boost::shared_ptr<Eigen::Quaternion<double> > pPoint);
    
    visualization_msgs::Marker generateArrowToPoint(const int pMarkerID, const std_msgs::ColorRGBA& pColor, const boost::shared_ptr<Eigen::Quaternion<double> > pPoint);
    
    void publishPointCloud(const int pMarkerID, const std::vector<boost::shared_ptr<Eigen::Quaternion<double> > > pCloud);
    
    visualization_msgs::Marker generatePointCloudPoint(const int pMarkerID, const unsigned int pId, const boost::shared_ptr<Eigen::Quaternion<double> >& pPoint);
    
    template <class T> void stlVectorToPointMsg(const std::vector<T> pPositionVector, geometry_msgs::Point& pROSPositionMsg);
    
  private:
    
    ros::NodeHandle mHandle;
    
    boost::shared_ptr<ros::Publisher> mPublisher;
    
    double mScale;
    
    boost::shared_ptr<Eigen::Vector3d> mMean;
    
    boost::shared_ptr<Eigen::Matrix3d> mCovariance;
    
    std::string mObjectLocationFrameID;
    
    float mAlphaChannel;
    
    float mSigmaMultiplicator;
  };
}