asrPosePredictionEngine.h

Go to the documentation of this file.

#pragma once

#include <boost/shared_ptr.hpp>
#include <rapidxml.hpp>
#include <fstream>
#include <cstddef>

#include "OcmSceneGraphNode.h"
#include "GMM.h"
#include <string>
#include <helper/ObjectTransformation.h>
#include <asr_msgs/AsrObject.h>
#include <asr_msgs/AsrAttributedPoint.h>

namespace ASR
{


using namespace rapidxml;
using namespace std;


class asrPosePredictionEngine
{
private:

    // path to xml file
    std::string path;
    // name of the reference object
    std::string reference_object;
    // the name of the current scene
    std::string scene_name;

    // the node of the reference object
    ASR::asrSceneGraphNodePtr root;

    int number_obsereved_objects;
    int number_objects_in_scene;
    int number_of_votes;                // overall available votes
    int votes_per_node;                 // votes that are available to one unobserved node
    //std::string pPbdObjectTopic;        // The ROS topic of obsererved Objects to listen to.
    //ros::Subscriber mObjectListener;    // A callback handler listening to objects found by an object detection system.
    std::string baseFrameId;            // The frame to which the geometry:msgs are transformed into

    // A transformer for objects into the target coordinate frame.
    ProbabilisticSceneRecognition::ObjectTransformation mObjectTransform;


    bool publish_hypothesis;
    bool print_debug_messages;
    std::string publish_hypothesis_topic;   // the topic to which the hypotheses are published
    ros::Publisher hypothesis_publisher;    // the publisher that publishs the generated hypothesis
    std::vector<boost::shared_ptr<asr_msgs::AsrObject>> evidence_buffer; // A buffer that stores all observed objects that occured since the last hypotheses update

public:

    asrPosePredictionEngine(std::string pathToXML,
                            std::string sceneName,
                            std::vector<boost::shared_ptr<asr_msgs::AsrObject>> objects,
                            int votes,
                            std::string base_frame);


    asrPosePredictionEngine(std::string pathToXML,
                            std::string sceneName,
                            std::string reference_object_name,
                            Eigen::Vector3f reference_object_position,
                            Eigen::Vector4f reference_object_orientation,
                            int votes,
                            std::string base_frame);


    unsigned int getNumberOfMissingObjects() {return this->number_objects_in_scene - this->number_obsereved_objects;}


    ASR::asrSceneGraphNodePtr getSceneGraphRoot() {return root;}


    void publishHypothesisMessages(std::vector<ASR::AttributedPoint> hypothesis_list);


    void collectPoseHypothesesRecursive(std::vector<ASR::AttributedPoint> &out_hypotheses,
                                        std::vector<ASR::AttributedPoint> &out_found_objects);


    void calcHypotheses();

private:

    unsigned int distributeVotes();

    bool observeObject(std::string object_name, Eigen::Vector3f position, Eigen::Vector4f orientation);

    void onObjectMsg(const boost::shared_ptr<asr_msgs::AsrObject>& pObject);


    bool transformObject(boost::shared_ptr<asr_msgs::AsrObject> in_out_object, std::string targetFrame);


    void init(std::string sceneName,
              std::string reference_object_name,
              Eigen::Vector3f reference_object_position,
              Eigen::Vector4f reference_object_orientation,
              std::string base_frame);


    void traverseSceneShapeRecursive(std::string parent_name, xml_node<> * parent_node, ASR::asrSceneGraphNodePtr root);


    void initVectorFromCSVString(std::vector<float> &x, std::string csv);


    MatrixXf initMatrixXf(std::vector<float> values, unsigned int rows, unsigned int cols);
};

typedef boost::shared_ptr<asrPosePredictionEngine> asrPosePredictionEnginePtr;

}