ProbabilisticSecondarySceneObjectVisualization.h

Go to the documentation of this file.

# pragma once

// Global includes
#include <map>
#include <utility>
#include <vector>
#include <math.h>

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

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

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

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

namespace Visualization
{
  class ProbabilisticSecondarySceneObjectVisualization
  {
  public:
    
    ProbabilisticSecondarySceneObjectVisualization();
    
    ProbabilisticSecondarySceneObjectVisualization(std::string pName);
    
    ~ProbabilisticSecondarySceneObjectVisualization();
    
    void setDrawingParameters(const double pScale, const float pSigmaMultiplicator, const std::string pFrameId);
    
    void setNamespace(std::string pNamespace);
    
    void setColors(const std_msgs::ColorRGBA& pFirstRingColor, const std_msgs::ColorRGBA& pSecondRingColor);
    
    void setDetectionCertainty(double pCertainty);
    
    void resetCertainty();
    
    void setLastPose(boost::shared_ptr<ISM::Pose> pPose);
    
    void setBestCandidatePose(boost::shared_ptr<ISM::Pose> pPose);
    
    void setBestPoseCandidate(double pScore);
    
    void setParentPose(boost::shared_ptr<ISM::Pose> pPose);
    
    void setAbsoluteLearningSamples(const std::vector<Eigen::Vector3d>& pRelativeSamples,
                                    const std::vector<Eigen::Vector3d>& pAbsoluteSamples,
                                    const std::vector<Eigen::Vector3d>& pAbsoluteParentSamples);
    
    void overwriteLastBestPose();
    
    void appendKernel(const boost::shared_ptr<Eigen::Vector3d>& pMean, const boost::shared_ptr<Eigen::Matrix3d>& pCovariance);
    
    void visualizeGaussianKernels(unsigned int& pMarkerId);
    
    void visualizeObjectPositionAsArrow(unsigned int& pMarkerId);
    
    void visualizeKinematicChain(unsigned int& pMarkerId);
    
    void visualizeRelativeTrajectory(unsigned int& pMarkerId);
    
    void visualizeAbsoluteTrajectory(unsigned int& pMarkerId);
    
  private:
    
    std::string mSceneObject;
    
    ros::NodeHandle mHandle;
    
    boost::shared_ptr<ros::Publisher> mPublisher;
    
    CoordinateFrameVisualizer visualizerFrame;
    
    GaussianKernelVisualizer visualizerKernel;
    
    KinematicChainVisualizer visualizerKinematics;
    
    SampleVisualizer visualizerSamples;
    
    double mCertainty;
    
    double mBestScore;
    
    double mBestScoreCandidate;
    
    boost::shared_ptr<ISM::Pose> mLastObjectPose;
    
    boost::shared_ptr<ISM::Pose> mBestPose;
    
    boost::shared_ptr<ISM::Pose> mBestCandidatePose;
    
    boost::shared_ptr<ISM::Pose> mParentPose;
    
    std::vector<std::pair<Eigen::Vector3d, Eigen::Matrix3d> > mKernels;
    
    std::vector<Eigen::Vector3d> mRelativeSamples, mAbsoluteSamples, mAbsoluteParentSamples;
  };
}