ProbabilisticPrimarySceneObjectVisualization.h

Go to the documentation of this file.

# pragma once

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

// Package includes
#include <ros/ros.h>
#include <std_msgs/ColorRGBA.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.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 <Pose.h>
#include <ISM/common_type/Pose.hpp>

// Local includes
#include "visualization/psm/ProbabilisticSecondarySceneObjectVisualization.h"

#include "visualization/psm/helper/CoordinateFrameVisualizer.h"
#include "visualization/psm/helper/KinematicChainVisualizer.h"
#include "visualization/psm/helper/SampleVisualizer.h"

#include "helper/ColorHelper.h"

namespace Visualization
{
  class ProbabilisticPrimarySceneObjectVisualization
  {
  public:
    
    ProbabilisticPrimarySceneObjectVisualization(std::string pName);
    
    ~ProbabilisticPrimarySceneObjectVisualization();
    
    void appendVisualizer(boost::shared_ptr<ProbabilisticSecondarySceneObjectVisualization> pVisualizer);
    
    void setDrawingParameters(const double pScale, const float pSigmaMultiplicator, const std::string pFrameId);
    
    void setPose(boost::shared_ptr<ISM::Pose> pPose);
    
    void setBestPoseCandidate(boost::shared_ptr<ISM::Pose> pPose);
    
    void setLearningSamples(const std::vector<Eigen::Vector3d>& pSamples);
    
    void drawInTargetingMode(const std::string pScene, const std_msgs::ColorRGBA& pColor);
    
    void drawInInferenceMode(const std::string pScene, const std_msgs::ColorRGBA& pColor, unsigned int& pMarkerId);
    
    void drawInLearningMode(const std::string pScene, const std_msgs::ColorRGBA& pColor);
    
    void drawInLearningTrajectoryMode(const std::string pScene);
    
    std::string getSceneObjectName();
    
    void addTermScore(double pScore);
    
    void resetTermScores();
    
    void setHypothesisScore(double pHypothesisScore);
    
    void normalizeHypothesisScore(unsigned int pNumberOfSlots);
    
    void setBestStatus(bool pStatus);
    
    void resetHypothesis();
    
  private:
    
    std::string mSceneObject;
    
    ros::NodeHandle mHandle;
    
    boost::shared_ptr<ros::Publisher> mPublisher;
    
    CoordinateFrameVisualizer visualizerFrame;
    
    KinematicChainVisualizer visualizerKinematics;
    
    SampleVisualizer visualizerSamples;
    
    boost::shared_ptr<ISM::Pose> mAbsolutePose;
    
    double mBestHypothesisScore;
    
    std::vector<double> mTermScores, mBestTermScores;
    
    double mAppearanceScore, mShapeScore, mExistenceScore;
    
    boost::shared_ptr<ISM::Pose> mBestPose;
    
    boost::shared_ptr<ISM::Pose> mBestCandidatePose;
    
    std::vector<boost::shared_ptr<ProbabilisticSecondarySceneObjectVisualization> > mSceneObjectVisualizers;
    
    std::vector<Eigen::Vector3d> mSamples;
  };
}