SceneLearningEngine.cpp

Go to the documentation of this file.

#include "learner/SceneLearningEngine.h"


namespace ProbabilisticSceneRecognition {



  SceneLearningEngine::SceneLearningEngine()
  : mPrivateNamespaceHandle("~")
  {

    // Volume of the workspace the scene takes place in.
    double workspaceVolume;
    
    // Parameters of heuristics used for hierarchical clustering.
    double staticBreakRatio, togetherRatio, maxAngleDeviation;
    
    // Try to get the name of the scene model.
    if(!mPrivateNamespaceHandle.getParam("scene_model_name", mSceneModelName))
      throw std::runtime_error("Please specify parameter " + std::string("scene_model_name") + " when starting this node.");
    
    // Try to get the type of the scene model representation.
    if(!mPrivateNamespaceHandle.getParam("scene_model_type", mSceneModelType))
      throw std::runtime_error("Please specify parameter " + std::string("scene_model_type") + " when starting this node.");
    
    // Try to get the directory the model should be stored.
    if(!mPrivateNamespaceHandle.getParam("scene_model_directory", mSceneModelDirectory))
      throw std::runtime_error("Please specify parameter " + std::string("scene_model_directory") + " when starting this node.");
    
    // Get the current date and time for the timestamp.
    std::stringstream ss;
    ss.imbue(std::locale(ss.getloc(), new boost::local_time::local_time_facet("%Y-%m-%d %H:%M:%S")));
    boost::local_time::time_zone_ptr zone_GMT1(new boost::local_time::posix_time_zone("GMT+1"));
    ss << boost::local_time::local_sec_clock::local_time(zone_GMT1);
    mDateTime = ss.str();
    

    // Try to get names of bag files with PbdSceneGraph message input, if any exist.
    if(!mPrivateNamespaceHandle.getParam("input_db_file", mInputDbFilename))
    {
        throw std::runtime_error("Please specify parameter " + std::string("input_db_file") + " when starting this node.");

    }
    
    // Try to get the volume of the workspace where the scene takes place.
    if(!mPrivateNamespaceHandle.getParam("workspace_volume", workspaceVolume))
      throw std::runtime_error("Please specify the 'workspace volume' when starting this node.");
    
    // Try to get the volume of the workspace where the scene takes place.
    if(!mPrivateNamespaceHandle.getParam("static_break_ratio", staticBreakRatio))
      throw std::runtime_error("Please specify the 'static_break_ratio' when starting this node.");
    
    // Try to get the volume of the workspace where the scene takes place.
    if(!mPrivateNamespaceHandle.getParam("together_ratio", togetherRatio))
      throw std::runtime_error("Please specify the 'together_ratio' when starting this node.");
    
    // Try to get the volume of the workspace where the scene takes place.
    if(!mPrivateNamespaceHandle.getParam("max_angle_deviation", maxAngleDeviation))
      throw std::runtime_error("Please specify the 'max_angle_deviation' when starting this node.");
    
    // Try to get the name of the scene to be published.
    if(!mPrivateNamespaceHandle.getParam("base_frame_id", mBaseFrameId))
       throw std::runtime_error("Please specify parameter " + std::string("base_frame_id") + " when starting this node.");
    
    // Try to get the visualization scale factor.
    if(!mPrivateNamespaceHandle.getParam("scale_factor", mScaleFactor))
       throw std::runtime_error("Please specify parameter " + std::string("scale_factor") + " when starting this node.");
    
    // Try to get the sigma multiplicator.
    if(!mPrivateNamespaceHandle.getParam("sigma_multiplicator", mSigmaMultiplicator))
       throw std::runtime_error("Please specify parameter " + std::string("sigma_multiplicator") + " when starting this node.");
    
    // Try to get the targeting help flag.
    if(!mPrivateNamespaceHandle.getParam("intermediate_results", mIntermediateResults))
       throw std::runtime_error("Please specify parameter " + std::string("intermediate_results") + " when starting this node.");
    
    // Try to get the add timestamp to filenames flag.
    if(!mPrivateNamespaceHandle.getParam("timestamps", mAddTimestampsToFilename))
       throw std::runtime_error("Please specify parameter " + std::string("timestamps") + " when starting this node.");
    
    // Initialize the scene model.
    initializeSceneModel(workspaceVolume, staticBreakRatio, togetherRatio, maxAngleDeviation);
  }
  
  SceneLearningEngine::~SceneLearningEngine()
  {

  }
  
  void SceneLearningEngine::readLearnerInput()
  {
    // If only one string is given to node, just use this as path to scene graphs.
    // Otherwise load a bunch of files and process input as it was one file.


      extractTracksFromDbFile(static_cast<std::string>(mInputDbFilename));


  }


  void SceneLearningEngine::extractTracksFromDbFile(const std::string& dbFileName)
  {


      ROS_INFO_STREAM("Pulling Objects from database " << dbFileName.c_str());
      tableHelper = ISM::TableHelperPtr(new ISM::TableHelper(dbFileName));

        for(auto patternName : tableHelper->getRecordedPatternNames())
        {
            ISM::RecordedPatternPtr pattern = tableHelper->getRecordedPattern(patternName);
            for(ISM::ObjectSetPtr objectSet : pattern->objectSets)
            {
                objectSet->mIdentifier = pattern->name;
                mSceneModelLearner->addExample(objectSet);
            }
        }



  }

  void SceneLearningEngine::generateSceneModel()
  {
    // Some error checking.
    if(!mSceneModelLearner)
      throw std::logic_error("Cannot save a non-existing scene model and plot its distributions.");
    
    // Calculates the scene model.
    mSceneModelLearner->generateSceneModel();
    
    // Initialize the visualizer.
    initializeVisualizationChain();
  }

  void SceneLearningEngine::saveSceneModel()
  {
    // Some error checking.
    if(!mSceneModelLearner)
      throw std::logic_error("Cannot save a non-existing scene model and plot its distributions.");

    // User information.
    ROS_INFO_STREAM("Scene model learner: Storing scene model '" << mSceneModelName << "'.");
    
    // Build the path to the model file based on the directory, the scene model name and the timestamp (if requried).
    std::string pathToFile = mSceneModelDirectory + "/";
    
    if(mAddTimestampsToFilename)
      pathToFile += mDateTime + "-";
    
    pathToFile += mSceneModelName + ".xml";
    
    // Write scene model to file.
    mSceneModelLearner->saveSceneModelToFile(pathToFile);
  }
  
  void SceneLearningEngine::visualizeSceneModel()
  {
    if(mVisualizer)
    {
      if(mIntermediateResults)
        mVisualizer->drawInLearningTrajectoryMode();
      else
        mVisualizer->drawInLearningMode();
    }
  }
  
  void SceneLearningEngine::initializeSceneModel(double pWorkspaceVolume, double mStaticBreakRatio, double mTogetherRatio, double mMaxAngleDeviation)
  {
    mSceneModelLearner.reset(new SceneModelLearner(mSceneModelType, pWorkspaceVolume, mStaticBreakRatio, mTogetherRatio, mMaxAngleDeviation));
  }
  
  void SceneLearningEngine::initializeVisualizationChain()
  {
    // Status information for the user.
    ROS_INFO("Initializing visualization mechanism.");
    
    // Create a new coordinator for scene visualization.
    mVisualizer.reset(new Visualization::ProbabilisticSceneModelVisualization());
    
    // Order the model to initialize the visualizers.
    mSceneModelLearner->initializeVisualizer(mVisualizer);
    
    // Set drawing parameters.
    mVisualizer->setDrawingParameters(mScaleFactor, mSigmaMultiplicator, mBaseFrameId);
  }
  
}