CDetectorDoorCrossing.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, 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::utils;
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
}