GaussianKernelVisualizer.h

Go to the documentation of this file.

# pragma once

// Global includes
#include <map>
#include <vector>

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

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

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

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <Pose.h>
#include <ISM/common_type/Pose.hpp>

// Local includes
#include "visualization/psm/helper/AbstractExtendedVisualizer.h"

namespace Visualization
{
  class GaussianKernelVisualizer : public AbstractExtendedVisualizer
  {
  public:
    
    GaussianKernelVisualizer();
    
    ~GaussianKernelVisualizer();
    
    void setSigmaMultiplicator(const float pSigmaMultiplicator);
    
    void setParentPose(boost::shared_ptr<ISM::Pose> pPose);
    
    void publishKernel(boost::shared_ptr<ros::Publisher> pPublisher,
                       unsigned int& pMarkerId,
                       const Eigen::Vector3d& pMean,
                       const Eigen::Matrix3d& pCovariance);
    
  private:
    
    float getSigmaMultiplicator();
    
    visualization_msgs::Marker generateAxis(const unsigned int pMarkerId,
                                             const unsigned int pAxis,
                                             const Eigen::Vector3d& pMean,
                                             const Eigen::Matrix3d& pCovariance);
    
    void calculateCoordinatePlaneEllipses(const std::vector<int>& pEllipsoidHalfAxes, const int pDiscretizationResolution, std::vector<std::vector<std::vector<int> > >& pEllipsePolygons);
    
    visualization_msgs::Marker generateCovarianceEllipsoidMarker(const unsigned int pMarkerId,
                                                                  const int pMarkerShape,
                                                                  const std::vector<std::vector<float> >& pWireFramePolygons,
                                                                  const std_msgs::ColorRGBA& pColor,
                                                                  const Eigen::Vector3d& pMean,
                                                                  const Eigen::Matrix3d& pCovariance);
    
    visualization_msgs::MarkerArray generateSamplesMarker(const unsigned int pMarkerId, const std::vector<boost::shared_ptr<Eigen::Quaternion<double> > >& pSamples);
    
    visualization_msgs::Marker generateArrowMessage(unsigned int& pMarkerId, Eigen::Vector3d pFrom, Eigen::Vector3d pTo);
    
    visualization_msgs::Marker generateSphereMessage(unsigned int& pMarkerId, Eigen::Vector3d pPos);
    
    template <class T> void stlVectorToPointMsg(const std::vector<T> pPositionVector, geometry_msgs::Point& pROSPositionMsg);
    
  private:
    
    float mSigmaMultiplicator;
    
    boost::shared_ptr<ISM::Pose> mParentPose;
  };
}