HierarchicalShapeModelNode.h

Go to the documentation of this file.

#pragma once

// Package includes
#include <Eigen/Core>
#include <Eigen/Geometry>

#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/property_tree/ptree.hpp>

#include <asr_msgs/AsrObject.h>

#include <Pose.h>

#include <visualization/psm/ProbabilisticPrimarySceneObjectVisualization.h>
#include <visualization/psm/ProbabilisticSecondarySceneObjectVisualization.h>

// Local includes
#include "inference/model/foreground/ocm/shape/GaussianMixtureDistribution.h"
#include <ISM/common_type/Pose.hpp>
#include <ISM/common_type/Object.hpp>

#include "inference/model/foreground/ocm/shape/ConditionalProbability.h"


namespace ProbabilisticSceneRecognition {
  
  class HierarchicalShapeModelNode {
  public:

    HierarchicalShapeModelNode(boost::property_tree::ptree& pPt, unsigned int& pID);
    
    ~HierarchicalShapeModelNode();
    
    void load(boost::property_tree::ptree& pPt, unsigned int& pID);  
 
    void initializeVisualizer(boost::shared_ptr<Visualization::ProbabilisticPrimarySceneObjectVisualization> mSuperior);
    
    void setAbsoluteParentPose(boost::shared_ptr<ISM::Pose> pPose);
    
    double calculateProbabilityForHypothesis(std::vector<ISM::Object> pEvidenceList, std::vector<unsigned int> pAssignments, unsigned int& pSlotId, bool pCut,
                        std::vector<boost::shared_ptr<ConditionalProbability>>& pConditionalProbabilities);
    
    void visualize(std::vector<ISM::Object> pEvidenceList);
    
    unsigned int getNumberOfNodes();

    bool isReference(unsigned int& pReferenceTo);

    std::vector<boost::shared_ptr<HierarchicalShapeModelNode>> getChildren();

    void setReferencedNode(boost::shared_ptr<HierarchicalShapeModelNode> pReferencedNode);

    void resetVisit();

    std::string getSceneObjectType() { return mSceneObject; }

    void setParentObjectType(const std::string& pParentObject) { mParentObject = pParentObject; }

  private:
    
    std::string mSceneObject;
    
    boost::shared_ptr<ISM::Pose> mAbsolutePose;
    
    boost::shared_ptr<ISM::Pose> mRelativePose;
    
    boost::shared_ptr<ISM::Pose> mAbsoluteParentPose;
    
    boost::shared_ptr<GaussianMixtureDistribution> mGaussianMixtureDistributionPosition;
    
    boost::shared_ptr<GaussianMixtureDistribution> mGaussianMixtureDistributionOrientation;
    
    std::vector<Eigen::Vector3d> mRawData;
    
    std::vector<boost::shared_ptr<HierarchicalShapeModelNode>> mChildren;
    
    boost::shared_ptr<Visualization::ProbabilisticSecondarySceneObjectVisualization> mVisualizer;

    bool mIsReference;
    unsigned int mReferenceTo;

    boost::shared_ptr<HierarchicalShapeModelNode> mReferencedNode;

    bool mWasVisited;

    std::string mParentObject;
  };
}