scene_recognition.h

Go to the documentation of this file.

#ifndef RECOGNIZER_PREDICTION_ISM_SCENE_RECOGNITION_NODE_H
#define RECOGNIZER_PREDICTION_ISM_SCENE_RECOGNITION_NODE_H

#include <iostream>
#include <fstream>
#include <cmath>

#include <ISM/recognizer/Recognizer.hpp>
#include <ISM/utility/TableHelper.hpp>
#include <ISM/common_type/ObjectSet.hpp>

/* ----------------- Foreign messages/services  ------------------  */
#include <std_srvs/Empty.h>

#include <asr_ism_visualizations/ObjectModelVisualizerRVIZ.hpp>
#include <asr_ism_visualizations/ism_result_visualizer_rviz.hpp>
#include <dynamic_reconfigure/server.h>
#include <asr_recognizer_prediction_ism/rp_ism_nodeConfig.h>

/* ----------------- Package messages/services  ------------------  */
#include <asr_world_model/CompletePattern.h>
#include <asr_world_model/VisualizeSampledPoses.h>
#include <asr_world_model/PushCompletePatterns.h>


#include "asr_recognizer_prediction_ism/FindScenes.h"
#include "asr_recognizer_prediction_ism/SetLogDir.h"
#include "asr_recognizer_prediction_ism/ToggleVisualization.h"

/* ----------------- Local includes ------------------  */
#include "shared_recognition_results_manager.h"
#include "ism_helper.h"

namespace recognizer_prediction_ism
{
  class SceneRecognition
  {
    /* ----------------- ROS ------------------  */
    ros::NodeHandle node_handle_;
    ros::ServiceServer find_scenes_server_;
    ros::ServiceServer set_log_dir_server_;
    ros::ServiceServer toggle_visualization_server_;
    ros::ServiceServer reset_server_;

    ros::ServiceClient viz_samples_client_;
    ros::ServiceClient push_complete_patterns_server_;

    /* ----------------- ISM ------------------  */
    ISM::RecognizerPtr recognizer_;
    ISM::TableHelperPtr table_helper_;
    double bin_size_;
    double max_projection_angle_deviation_;
    std::string database_filename_;
    unsigned int rater_type_;

    std::string object_constellation_folder_path_;
    int object_constellation_file_counter_;

    unsigned int object_set_max_count_;

    IH::ObjectConverterPtr converter;

    //Settings for handling of rotation invariant objects
    int enable_rotation_mode_;
    std::string rotation_frame_;
    std::string rotation_object_type_;
    std::string rotation_object_id_;

    //Size of last object set, on which find_scenes() has been performed.
    unsigned int last_object_set_size_;
    //Buffer shared with pose prediction.
    std::vector<ISM::RecognitionResultPtr>& results_already_shared_;

    //Mapping from patterns to objects from loaded sqlite file.
    void constructPatternObjectMapping();

    std::map<std::string, std::set<std::pair<std::string, std::string> > > pattern_object_map_;
    std::set<std::string> unique_pattern_names_;

    /* ----------------- Visualization ------------------  */
    std::string scene_markers_publisher_name_;
    std::string viz_samples_client_name_;
    std::string push_complete_patterns_name_;

    double marker_life_time_;
    double step_;
    std::string base_frame_;
    bool is_visualization_active_;
    VIZ::ISMResultVisualizerRVIZPtr res_visualizer_;
    VIZ::ObjectModelVisualizerRVIZ* object_model_visualizer_;
    ros::Publisher visualization_publisher_;
    ros::Publisher my_publisher_;
    SharedRecognitionResultsManagerPtr shared_recognition_results_ptr_;

    std::vector<ISM::RecognitionResultPtr> incomplete_recognition_results_;
    std::vector<ISM::RecognitionResultPtr> results_for_visualization_;
    std::set<std::pair<std::string, std::string> > object_types_and_ids_set_;

    /* ----------------- Help functions ------------------  */
    std::vector<ISM::ObjectPtr> extractRealObjects(ISM::RecognitionResultPtr result_ptr);

    static bool compareConfidence(ISM::RecognitionResultPtr i,ISM::RecognitionResultPtr j);
    static bool isCompleteSubset(std::set<std::pair<std::string, std::string> > set_a,
                 std::set<std::pair<std::string, std::string> > set_b);

  public:

    //TODO: Comment.
    SceneRecognition(std::vector<ISM::RecognitionResultPtr>& results_already_shared, SharedRecognitionResultsManagerPtr shared_recognition_results_ptr);

    ~SceneRecognition() { }

    bool processFindScenesServiceCall(asr_recognizer_prediction_ism::FindScenes::Request &request, asr_recognizer_prediction_ism::FindScenes::Response &response);

    bool setLogFilePathServiceCall(asr_recognizer_prediction_ism::SetLogDir::Request &request, asr_recognizer_prediction_ism::SetLogDir::Response &response);

    bool processToggleVisualizationServiceCall(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response);

    bool processResetServiceCall(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response);

  private:
    void createObjectSetsFromPoseSamples(std::vector<asr_msgs::AsrObject>::iterator object_it, std::vector<asr_msgs::AsrObject>::iterator end_it,
                     ISM::ObjectSetPtr temp_set_ptr, std::vector<ISM::ObjectSetPtr>& object_sets, bool measured_value_only);
    void writeObjectConstellationToXML(std::vector<ISM::ObjectPtr> &objects, int sceneRecogCount, int constellation_count);

    void filterIncompleteRR(std::vector<ISM::RecognitionResultPtr> &recognition_results);
    void sortBestRRperScene(std::stringstream& output);
    void deleteAllNewRR();

  };

  typedef boost::shared_ptr<SceneRecognition> SceneRecognitionPtr;

}
#endif