CObservationBearingRange.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 "obs-precomp.h"  // Precompiled headers
 
#include <mrpt/serialization/CArchive.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::poses;
using namespace mrpt::math;
 
// 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)
{
}
 
uint8_t CObservationBearingRange::serializeGetVersion() const { return 3; }
void CObservationBearingRange::serializeTo(
        mrpt::serialization::CArchive& out) const
{
        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;
}
 
void CObservationBearingRange::serializeFrom(
        mrpt::serialization::CArchive& in, uint8_t 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)
        };
}
 
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<CMatrixDouble44>()
          << 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";
        }
}