ISMVisualization.h

Go to the documentation of this file.

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

#include <string>
#include <set>
#include <utility>

#include <boost/shared_ptr.hpp>

#include <tf/transform_listener.h>

#include <Eigen/Core>

#include <ISM/RecognitionResult.hpp>

#include "QuadGrid.h"

namespace Visualization {
  static const int ID_PTU_FOV = 0;
  static const int ID_TESSELLATED_SPHERE = 1;
  static const int ID_SAMPLES_2D = 2;
  static const int ID_SAMPLES_3D = 3;
  static const int ID_QUADS = 20000;

  static const int GRID_SCALE = 0.005;
  static const int TS_RADIUS = 0.5:

  class ISMVisualization {
        
  private:
    // The grid containing the votes
    QuadGridPtr grid;

    // Resolution for the tesselated sphere
    double quad_pan_deg;
    double quad_tilt_deg;

    // Publishers for vsualization markers
    ros::Publisher ismPublisher;
    ros::Publisher arrowPublisher;
    ros::Publisher tsPublisher;
    ros::Publisher qgPublisher;
    ros::Publisher fovPublisher;
    ros::Publisher samples2dPublisher;
    ros::Publisher samples3dPublisher;
        
  public:
    ISMVisualization();
    ~ISMVisualization();

    // Draw tesselated sphere as grid
    void drawTesselatedSphere(float panMin, float panMax, float tiltMin, float tiltMax);
                
    // Draws extent of camera view with current PTU state
    void drawFoV(float pan, float tilt, float foVX, float foVY);

    // Draws relations and so on in one recognized scene given as parameter
    void drawRecognitionResult(ISM::RecognitionResultPtr result, int level, ISM::PointPtr parentPoint, std::string prefix, unsigned char action);
                
    // Draws color coded quad ratings
    void drawQuads(float panMin, float panMax, float tiltMin, float tiltMax);

    // Draws object position samples in 3D space and their projection on tesselated sphere
    void drawSamples();

    // Draw arrows from scene reference to object position votes for objects belonging to scene and not found until yet
    void drawUnfoundObjectsVotes();

  private:

    // Adds the given distance to the z-Axis of the point
    ISM::PointPtr applyLevelToPoint(ISM::PointPtr p, int level);
    
    // Creates a visualization message that draws a point as sphere
    visualization_msgs::Marker pointToSphere(ISM::PointPtr point, unsigned char action);
                
    // Creates a visualization message that draws a line between points
    visualization_msgs::Marker lineBetweenPoints(ISM::PointPtr point1, ISM::PointPtr point2, unsigned char action);
        
    // Creates a visualization message that draws an arrow between points
    visualization_msgs::Marker arrowBetweenPoints(const ISM::PointPtr& point1, const ISM::PointPtr& point2, unsigned char action);

    // Creates a visualization message that draws an arrow between points
    visualization_msgs::Marker arrowBetweenPoints2(const ISM::PointPtr& point1, const ISM::PointPtr& point2);
                
    // Creates a geometry message containing a point. Helper function for methods above.
    geometry_msgs::Point pointToPointMsg(ISM::PointPtr point);
                
    // Creates the appropriate color for a given confidence level
    std_msgs::ColorRGBA confidenceToColor(double confidence);
                
    // Calculates the angle to the PTU.
    geometry_msgs::Point angleToPTU(float pan, float tilt, float r);

    // Returns the Quaternion that belongs to the given angle
    tf::Quaternion angleToQuaternion(float pan, float tilt);

    // ???
    Eigen::Vector2f getQuadExtent(Eigen::Vector2f pos, float r, float panSize = 5, float tiltSize = 5);
  };

  typedef boost::shared_ptr<ISMVisualization> ISMVisualizationPtr;
}