ProbabilisticSceneVisualization.cpp

Go to the documentation of this file.

#include "visualization/psm/ProbabilisticSceneVisualization.h"

namespace Visualization {
        
  ProbabilisticSceneVisualization::ProbabilisticSceneVisualization(std::string pScene)
  : mScene(pScene)
  {
  }
        
  ProbabilisticSceneVisualization::~ProbabilisticSceneVisualization()
  {
  }
  
  void ProbabilisticSceneVisualization::appendVisualizer(boost::shared_ptr<ProbabilisticPrimarySceneObjectVisualization> pVisualizer)
  {
    mSceneObjectVisualizers.push_back(pVisualizer);
  }
  
  void ProbabilisticSceneVisualization::setDrawingParameters(const double pScale, const float pSigmaMultiplicator, const std::string pFrameId)
  {
    for(boost::shared_ptr<ProbabilisticPrimarySceneObjectVisualization> visualizer : mSceneObjectVisualizers)
      visualizer->setDrawingParameters(pScale, pSigmaMultiplicator, pFrameId);
  }
  
  void ProbabilisticSceneVisualization::drawInTargetingMode()
  {
    // Stop, if no primary scene object visualizers registered.
    if(mSceneObjectVisualizers.size() == 0)
      return;
    
    // The size of a step in the hsv color space.
    double hsvColorSteps = 1.0 / mSceneObjectVisualizers.size();
    
    // Iterate over all scene objects and create a color for each one.
    // But draw only the scene object with the given name.
    for(unsigned int i = 0; i < mSceneObjectVisualizers.size(); i++)
    {
      // Create an unique color...
      std_msgs::ColorRGBA color;
      color.a = 1.0;
      
      // ... by converting an index based postion on the hue axis into RGB
      ColorHelper::convertHSVToRGB(color, (double) hsvColorSteps * i, 1.0, 1.0);
      
      // Draw the scene.
      mSceneObjectVisualizers[i]->drawInTargetingMode(mScene, color);
    }
  }
  
  void ProbabilisticSceneVisualization::drawInInferenceMode()
  {
    // Stop, if no primary scene object visualizers registered.
    if(mSceneObjectVisualizers.size() == 0)
      return;
    
    // The size of a step in the hsv color space.
    double hsvColorSteps = 1.0 / mSceneObjectVisualizers.size();
    
    // Serves as unique id for the marker. Any marker sent with the same id will overwrite the old one.
    unsigned int markerId = 0;
    
    // Iterate over all scene objects and create a color for each one.
    // But draw only the scene object with the given name.
    for(unsigned int i = 0; i < mSceneObjectVisualizers.size(); i++)
    {
      // Create an unique color...
      std_msgs::ColorRGBA color;
      color.a = 1.0;
      
      // ... by converting an index based postion on the hue axis into RGB
      ColorHelper::convertHSVToRGB(color, (double) hsvColorSteps * i, 1.0, 1.0);
      
      // Draw the scene.
      mSceneObjectVisualizers[i]->drawInInferenceMode(mScene, color, markerId);
    }
  }
  
  void ProbabilisticSceneVisualization::drawInLearningMode()
  {
    // Stop, if no primary scene object visualizers registered.
    if(mSceneObjectVisualizers.size() == 0)
      return;
    
    // The size of a step in the hsv color space.
    double hsvColorSteps = 1.0 / mSceneObjectVisualizers.size();
    
    // Iterate over all scene objects and create a color for each one.
    // But draw only the scene object with the given name.
    for(unsigned int i = 0; i < mSceneObjectVisualizers.size(); i++)
    {
      // Create an unique color...
      std_msgs::ColorRGBA color;
      color.a = 1.0;
      
      // ... by converting an index based postion on the hue axis into RGB
      ColorHelper::convertHSVToRGB(color, (double) hsvColorSteps * i, 1.0, 1.0);
      
      // Draw the scene.
      mSceneObjectVisualizers[i]->drawInLearningMode(mScene, color);
    }
  }
  
  void ProbabilisticSceneVisualization::drawInLearningTrajectoryMode()
  {
    for(unsigned int i = 0; i < mSceneObjectVisualizers.size(); i++)
      mSceneObjectVisualizers[i]->drawInLearningTrajectoryMode(mScene);
  }
  
}