CDetectorDoorCrossing.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
 
#include "detectors-precomp.h"  // Precompiled headers
 
#include <mrpt/detectors/CDetectorDoorCrossing.h>
#include <mrpt/maps/CMultiMetricMap.h>
#include <mrpt/poses/CPosePDF.h>
 
using namespace mrpt;
using namespace mrpt::obs;
using namespace mrpt::maps;
using namespace mrpt::detectors;
using namespace mrpt::poses;
 
/*---------------------------------------------------------------
                        Constructor
  ---------------------------------------------------------------*/
CDetectorDoorCrossing::CDetectorDoorCrossing()
    : COutputLogger("CDetectorDoorCrossing"),
      options(),
      lastObs(),
      entropy(),
      lastEntropy(),
      lastEntropyValid(false)
{
    clear();
}
 
/*---------------------------------------------------------------
                        clear
  ---------------------------------------------------------------*/
void CDetectorDoorCrossing::clear()
{
    lastObs.clear();
    lastEntropyValid = false;
}
 
/*---------------------------------------------------------------
                        process
  ---------------------------------------------------------------*/
void CDetectorDoorCrossing::process(
    CActionRobotMovement2D& in_poseChange, CSensoryFrame& in_sf,
    TDoorCrossingOutParams& out_estimation)
{
    // Variables for generic use:
    size_t i;
 
    out_estimation.cumulativeTurning = 0;
 
    MRPT_START
 
    // 1) Add new pair to the list:
    // -----------------------------------------
    lastObs.addAction(in_poseChange);
    lastObs.addObservations(in_sf);
 
    // 2) Remove oldest pair:
    // -----------------------------------------
    ASSERT_(options.windowSize > 1);
    ASSERT_((lastObs.size() % 2) == 0);  // Assure even size
 
    while (lastObs.size() > options.windowSize * 2)
    {
        lastObs.remove(0);
        lastObs.remove(0);
    }
 
    if (lastObs.size() < options.windowSize * 2)
    {
        // Not enought old data yet:
        out_estimation.enoughtInformation = false;
        return;
    }
 
    // 3) Build an occupancy grid map with observations
    // -------------------------------------------------
    CPose2D p, pos;
 
    TSetOfMetricMapInitializers mapInitializer;
 
    {
        CSimplePointsMap::TMapDefinition def;
        mapInitializer.push_back(def);
    }
    {
        COccupancyGridMap2D::TMapDefinition def;
        def.resolution = options.gridResolution;
        mapInitializer.push_back(def);
    }
 
    CMultiMetricMap auxMap(&mapInitializer);
 
    for (i = 0; i < options.windowSize; i++)
    {
        CActionCollection::Ptr acts = lastObs.getAsAction(i * 2 + 0);
        CAction::Ptr act = acts->get(0);
 
        ASSERT_(
            act->GetRuntimeClass()->derivedFrom(
                CLASS_ID(CActionRobotMovement2D)));
        CActionRobotMovement2D::Ptr action =
            std::dynamic_pointer_cast<CActionRobotMovement2D>(act);
 
        action->poseChange->getMean(pos);
 
        out_estimation.cumulativeTurning += fabs(pos.phi());
 
        // Get the cumulative pose for the current observation:
        p = p + pos;
 
        // Add SF to the grid map:
        CSensoryFrame::Ptr sf = lastObs.getAsObservations(i * 2 + 1);
        CPose3D pose3D(p);
        sf->insertObservationsInto(&auxMap, &pose3D);
    }
 
    // 4) Compute the information differece between this
    //      "map patch" and the previous one:
    // -------------------------------------------------------
    auxMap.m_gridMaps[0]->computeEntropy(entropy);
 
    if (!lastEntropyValid)
    {
        out_estimation.enoughtInformation = false;
    }
    else
    {
        // 5) Fill output data
        // ---------------------------------
        out_estimation.enoughtInformation = true;
 
        out_estimation.informationGain = entropy.I - lastEntropy.I;
        out_estimation.pointsMap.copyFrom(*auxMap.m_pointsMaps[0]);
    }
 
    // For next iterations:
    lastEntropy = entropy;
    lastEntropyValid = true;
 
    MRPT_END
}