CLandmark.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 "vision-precomp.h"   // Precompiled headers

#include <mrpt/utils/CStream.h>
#include <mrpt/maps/CLandmark.h>
#include <mrpt/obs/CObservation.h>
#include <mrpt/utils/stl_serialization.h>

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


IMPLEMENTS_SERIALIZABLE(CLandmark, CSerializable,mrpt::maps)

// Initialization:
CLandmark::TLandmarkID  CLandmark::m_counterIDs= static_cast<CLandmark::TLandmarkID>(0);

/*---------------------------------------------------------------
                                                Default constructor
  ---------------------------------------------------------------*/
CLandmark::CLandmark( ) :
        features(),
        pose_mean(),
        normal(),
        pose_cov_11(),pose_cov_22(),pose_cov_33(),pose_cov_12(),pose_cov_13(),pose_cov_23(),
        ID( INVALID_LANDMARK_ID ),
    timestampLastSeen( INVALID_TIMESTAMP ),
        seenTimesCount(0)
{
}

/*---------------------------------------------------------------
                                                Destructor
  ---------------------------------------------------------------*/
CLandmark::~CLandmark()
{

}

/*---------------------------------------------------------------
                                                Destructor
  ---------------------------------------------------------------*/
void    CLandmark::getPose( CPointPDFGaussian   &pose ) const
{
        pose.mean.x( pose_mean.x);
        pose.mean.y( pose_mean.y);
        pose.mean.z( pose_mean.z);

        pose.cov(0,0) = pose_cov_11;
        pose.cov(1,1) = pose_cov_22;
        pose.cov(2,2) = pose_cov_33;

        pose.cov(0,1) = pose.cov(1,0) = pose_cov_12;
        pose.cov(0,2) = pose.cov(2,0) = pose_cov_13;

        pose.cov(1,2) = pose.cov(2,1) = pose_cov_23;
}

/*---------------------------------------------------------------
                                                Destructor
  ---------------------------------------------------------------*/
void    CLandmark::setPose( const CPointPDFGaussian     &pose )
{
        pose_mean.x = pose.mean.x();
        pose_mean.y = pose.mean.y();
        pose_mean.z = pose.mean.z();

        pose_cov_11 = pose.cov(0,0);
        pose_cov_22 = pose.cov(1,1);
        pose_cov_33 = pose.cov(2,2);
        pose_cov_12 = pose.cov(0,1);
        pose_cov_13 = pose.cov(0,2);
    pose_cov_23 = pose.cov(1,2);
}

/*---------------------------------------------------------------
                                        writeToStream
   Implements the writing to a CStream capability of
     CSerializable objects
  ---------------------------------------------------------------*/
void  CLandmark::writeToStream(mrpt::utils::CStream &out, int *version) const
{
        if (version)
                *version = 4;
        else
        {
                out << features
                        << pose_mean
                        << normal
                        << pose_cov_11 << pose_cov_22 << pose_cov_33
                        << pose_cov_12 << pose_cov_13 << pose_cov_23
                        << ID
                        << timestampLastSeen
                        << seenTimesCount;
        }
}

/*---------------------------------------------------------------
                                        readFromStream
   Implements the reading from a CStream capability of
      CSerializable objects
  ---------------------------------------------------------------*/
void  CLandmark::readFromStream(mrpt::utils::CStream &in, int version)
{
        switch(version)
        {
        case 0:
        case 1:
        case 2:
        case 3:
                THROW_EXCEPTION("Importing from this old version is not implemented");
                break;
        case 4:
                {
                in  >> features
                        >> pose_mean
                        >> normal
                        >> pose_cov_11 >> pose_cov_22 >> pose_cov_33
                        >> pose_cov_12 >> pose_cov_13 >> pose_cov_23
                        >> ID
                        >> timestampLastSeen
                        >> seenTimesCount;
                } break;
        default:
                MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
        };
}