CObservationBearingRange.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 "obs-precomp.h"  // Precompiled headers

#include <mrpt/utils/CStream.h>
#include <mrpt/obs/CObservationBearingRange.h>
#include <mrpt/system/os.h>
#include <mrpt/math/matrix_serialization.h>  // for << ops
#include <mrpt/math/wrap2pi.h>
#include <set>

using namespace mrpt::obs;
using namespace mrpt::utils;
using namespace mrpt::poses;

// This must be added to any CSerializable class implementation file.
IMPLEMENTS_SERIALIZABLE(CObservationBearingRange, CObservation, mrpt::obs)

/*---------------------------------------------------------------
 Default constructor.
 ---------------------------------------------------------------*/
CObservationBearingRange::CObservationBearingRange()
        : minSensorDistance(0),
          maxSensorDistance(0),
          fieldOfView_yaw(DEG2RAD(180)),
          fieldOfView_pitch(DEG2RAD(90)),
          sensorLocationOnRobot(),
          sensedData(),
          validCovariances(false),
          sensor_std_range(0),
          sensor_std_yaw(0),
          sensor_std_pitch(0)
{
}

/*---------------------------------------------------------------
  Implements the writing to a CStream capability of CSerializable objects
 ---------------------------------------------------------------*/
void CObservationBearingRange::writeToStream(
        mrpt::utils::CStream& out, int* version) const
{
        if (version)
                *version = 3;
        else
        {
                uint32_t i, n;

                // The data
                out << minSensorDistance << maxSensorDistance << fieldOfView_yaw
                        << fieldOfView_pitch << sensorLocationOnRobot << timestamp;

                out << validCovariances;
                if (!validCovariances)
                        out << sensor_std_range << sensor_std_yaw << sensor_std_pitch;

                // Detect duplicate landmarks ID, which is an error!
                std::set<int32_t> lstIDs;

                n = sensedData.size();
                out << n;
                for (i = 0; i < n; i++)
                {
                        int32_t id = sensedData[i].landmarkID;
                        if (id != INVALID_LANDMARK_ID)
                        {
                                if (0 != lstIDs.count(id))
                                        THROW_EXCEPTION_FMT(
                                                "Duplicate landmark ID=%i found.", (int)id);
                                lstIDs.insert(id);
                        }

                        out << sensedData[i].range << sensedData[i].yaw
                                << sensedData[i].pitch << id;

                        if (validCovariances) out << sensedData[i].covariance;
                }

                out << sensorLabel;
        }
}

/*---------------------------------------------------------------
  Implements the reading from a CStream capability of CSerializable objects
 ---------------------------------------------------------------*/
void CObservationBearingRange::readFromStream(
        mrpt::utils::CStream& in, int version)
{
        switch (version)
        {
                case 0:
                case 1:
                case 2:
                case 3:
                {
                        uint32_t i, n;

                        // The data
                        in >> minSensorDistance >> maxSensorDistance;

                        if (version >= 3)
                        {
                                in >> fieldOfView_yaw >> fieldOfView_pitch;
                        }
                        else
                        {
                                float fieldOfView;
                                in >> fieldOfView;

                                fieldOfView_yaw = fieldOfView_pitch = fieldOfView;
                        }

                        in >> sensorLocationOnRobot;

                        if (version >= 2)
                                in >> timestamp;
                        else
                                timestamp = INVALID_TIMESTAMP;

                        if (version >= 3)
                        {
                                in >> validCovariances;
                                if (!validCovariances)
                                        in >> sensor_std_range >> sensor_std_yaw >>
                                                sensor_std_pitch;
                        }
                        else
                                validCovariances = false;

                        in >> n;
                        sensedData.resize(n);

                        // Detect duplicate landmarks ID, what is an error!
                        std::set<int32_t> lstIDs;

                        for (i = 0; i < n; i++)
                        {
                                in >> sensedData[i].range >> sensedData[i].yaw >>
                                        sensedData[i].pitch >> sensedData[i].landmarkID;

                                if (version >= 3 && validCovariances)
                                        in >> sensedData[i].covariance;

                                int32_t id = sensedData[i].landmarkID;
                                if (id != INVALID_LANDMARK_ID)
                                {
                                        if (0 != lstIDs.count(id))
                                                THROW_EXCEPTION_FMT(
                                                        "Duplicate landmark ID=%i found.", (int)id);
                                        lstIDs.insert(id);
                                }
                        }

                        if (version >= 1)
                                in >> sensorLabel;
                        else
                                sensorLabel = "";
                }
                break;
                default:
                        MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
        };
}

/*---------------------------------------------------------------
  Implements the writing to a CStream capability of CSerializable objects
 ---------------------------------------------------------------*/
void CObservationBearingRange::debugPrintOut()
{
        printf("[CObservationBearingRange::debugPrintOut] Dumping:\n");
        printf(
                "[CObservationBearingRange::debugPrintOut] minSensorDistance:\t%f\n",
                minSensorDistance);
        printf(
                "[CObservationBearingRange::debugPrintOut] maxSensorDistance:\t%f:\n",
                maxSensorDistance);
        printf(
                "[CObservationBearingRange::debugPrintOut] %u landmarks:\n",
                static_cast<unsigned>(sensedData.size()));

        size_t i, n = sensedData.size();
        for (i = 0; i < n; i++)
                printf(
                        "[CObservationBearingRange::debugPrintOut] \tID[%i]: y:%fdeg "
                        "p:%fdeg range: %f\n",
                        sensedData[i].landmarkID, RAD2DEG(sensedData[i].yaw),
                        RAD2DEG(sensedData[i].pitch), sensedData[i].range);
}

void CObservationBearingRange::getDescriptionAsText(std::ostream& o) const
{
        using namespace std;
        CObservation::getDescriptionAsText(o);

        o << "Homogeneous matrix for the sensor's 3D pose, relative to robot "
                 "base:\n";
        o << sensorLocationOnRobot.getHomogeneousMatrixVal()
          << sensorLocationOnRobot << endl
          << endl;

        o << "Do observations have individual covariance matrices? "
          << (validCovariances ? "YES" : "NO") << endl
          << endl;

        o << "Default noise sigmas:" << endl;
        o << "sensor_std_range (m)   : " << sensor_std_range << endl;
        o << "sensor_std_yaw   (deg) : " << RAD2DEG(sensor_std_yaw) << endl;
        o << "sensor_std_pitch (deg) : " << RAD2DEG(sensor_std_pitch) << endl;

        o << endl;

        // For each entry in this sequence:
        o << "  LANDMARK_ID    RANGE (m)    YAW (deg)    PITCH (deg)   COV. MATRIX "
                 "(optional)"
          << endl;
        o << "---------------------------------------------------------------------"
                 "-----------------"
          << endl;
        for (size_t q = 0; q < sensedData.size(); q++)
        {
                o << "      ";
                if (sensedData[q].landmarkID == INVALID_LANDMARK_ID)
                        o << "(NO ID)";
                else
                        o << format("%7u", sensedData[q].landmarkID);

                o << format(
                        "   %10.03f  %10.03f %10.03f        ", sensedData[q].range,
                        RAD2DEG(mrpt::math::wrapToPi(sensedData[q].yaw)),
                        RAD2DEG(mrpt::math::wrapToPi(sensedData[q].pitch)));

                if (validCovariances)
                        o << sensedData[q].covariance.inMatlabFormat() << endl;
                else
                        o << "  (N/A)\n";
        }
}