Object.cpp

Go to the documentation of this file.

//
// Object.cpp
//

#include "Object.hpp"
#include "../tools/errorhandler.hpp"
#include "../tools/MathToolbox.hpp"     // for NaN_double

namespace datatypes
{


Object::Object()
        : m_objectId(0)
        , m_flags(0)
        , m_objectAge(0)
        , m_hiddenStatusAge(0)
        , m_timestamp()
        , m_classification(Object::Unclassified)
        , m_classificationAge(0)
        , m_classificationQuality(0.0f)
        , m_centerPoint(Point2D(0, 0))
        , m_centerPointSigma(Point2D(0, 0))
        , m_courseAngle(0.0f)
        , m_courseAngleSigma(0.0f)
        , m_relativeVelocity(Point2D(0, 0))
        , m_relativeVelocitySigma(Point2D(0, 0))
        , m_absoluteVelocity(Point2D(0, 0))
        , m_absoluteVelocitySigma(Point2D(0, 0))
        , m_objectBox(Point2D(0, 0))
        , m_objectBoxSigma(Point2D(0, 0))
        , m_boundingBoxCenter(Point2D(0, 0))
        , m_boundingBox(Point2D(0, 0))
        , m_closestPoint(Point2D(0, 0))
        , m_contourPoints()
        , m_vehicleWLANid(0)
        , m_objectHeight(0)
        , m_objectHeightSigma(0)
        , m_objectMass(0)
        , m_maxAbsoluteVelocity(NaN_double)
        , m_normalizedMeanPointDist(0)
        , m_totalTrackingDuration(0)
        , m_totalTrackedPathLength(0)
        , m_isValid(true)
{
}

Object::~Object()
{
}

bool Object::operator==(const Object& other) const
{
        bool b =
                m_objectId == other.m_objectId
                && m_flags == other.m_flags
                && m_objectAge == other.m_objectAge
                && m_hiddenStatusAge == other.m_hiddenStatusAge
                && m_timestamp == other.m_timestamp
                && m_classification == other.m_classification
                && m_classificationAge == other.m_classificationAge
                && m_classificationQuality == other.m_classificationQuality
                && m_centerPoint == other.m_centerPoint
                && m_centerPointSigma == other.m_centerPointSigma
                && m_courseAngle == other.m_courseAngle
                && m_courseAngleSigma == other.m_courseAngleSigma
                && m_relativeVelocity == other.m_relativeVelocity
                && m_relativeVelocitySigma == other.m_relativeVelocitySigma
                && m_absoluteVelocity == other.m_absoluteVelocity
                && m_absoluteVelocitySigma == other.m_absoluteVelocitySigma
                && m_objectBox == other.m_objectBox
                && m_objectBoxSigma == other.m_objectBoxSigma
                && m_boundingBoxCenter == other.m_boundingBoxCenter
                && m_boundingBox == other.m_boundingBox
                && m_closestPoint == other.m_closestPoint
                && m_contourPoints == other.m_contourPoints
                && m_objectHeight == other.m_objectHeight
                && m_objectHeightSigma == other.m_objectHeightSigma
                && m_objectMass == other.m_objectMass
                ;
        return b;
}


Box2D Object::getBox() const
{
        return Box2D(getCenterPoint(), getObjectBox(), getCourseAngle());
}

// The set-functions with checks for value ranges
void Object::setClassificationQuality(double v)
{
        assert(v >= 0);
        assert(v <= 1);
        m_classificationQuality = v;
}

void Object::setCenterPointSigma(const Point2D& v)
{
        assert((v.getX() >= 0) || isNaN(v.getX()));
        assert((v.getY() >= 0) || isNaN(v.getY()));
        m_centerPointSigma = v;
}

void Object::setCourseAngle(double new_angle)
{
        double normd_angle = normalizeRadians(new_angle);
        if (!fuzzyCompare(normd_angle, new_angle))
        {
                printWarning("Object::setCourseAngle was called with angle " + ::toString(new_angle, 2) +
                                                " which is outside of its [-pi,pi] definition range (but the calling function should have ensured to normalize into that interval) - normalizing it back into that range now (" +
                                                ::toString(normd_angle, 2) + ").");
                new_angle = normd_angle;
                // Note: We intentionally do not throw an exception here
                // because erroneous normalizations might occur very seldomly
                // (especially the exact value of +PI), hence might go through
                // development unnoticed, but an exception causes a crash in
                // front of the customer, which is bad. Also, we *do* print
                // the warning even though we also normalize it so that the
                // developers have a chance to notice this problem and fix
                // their calling code.
        }
        m_courseAngle = new_angle;
}

void Object::setCourseAngleSigma(double v)
{
        assert(v >= 0);
        m_courseAngleSigma = v;
}

void Object::setAbsoluteVelocity(const Point2D&  v)
{
        m_absoluteVelocity = v;
        double absolute = v.dist();
        if (isNaN(getMaxAbsoluteVelocity())
                || absolute > getMaxAbsoluteVelocity())
        {
                setMaxAbsoluteVelocity(absolute);
        }
}

void Object::setAbsoluteVelocitySigma(const Point2D& v)
{
        assert((v.getX() >= 0) || isNaN(v.getX()));
        assert((v.getY() >= 0) || isNaN(v.getY()));
        m_absoluteVelocitySigma = v;
}

void Object::setObjectBox(const Point2D& v)
{
        assert((v.getX() >= 0) || isNaN(v.getX()));
        assert((v.getY() >= 0) || isNaN(v.getY()));
        m_objectBox = v;
}

void Object::setObjectBoxSigma(const Point2D& v)
{
        assert((v.getX() >= 0) || isNaN(v.getX()));
        assert((v.getY() >= 0) || isNaN(v.getY()));
        m_objectBoxSigma = v;
}

void Object::setBoundingBox(const Point2D& v)
{
        assert((v.getX() >= 0) || isNaN(v.getX()));
        assert((v.getY() >= 0) || isNaN(v.getY()));
        m_boundingBox = v;
}

void Object::setBoundingBoxCenter(const Point2D& v)
{
        m_boundingBoxCenter = v;
}

void Object::setObjectHeightSigma(double v)
{
        assert(v >= 0);
        m_objectHeightSigma = v;
}

void Object::setObjectMass(double v)
{
        assert(v >= 0);
        m_objectMass = v;
}

void Object::setContourPoints(const Polygon2D& v)
{
        assert(v.size() <= 0xff);
        m_contourPoints = v;
}

void Object::addContourPoint(const Point2D cp)
{
        assert(m_contourPoints.size() < 0xff);
        m_contourPoints.push_back(cp);
}


std::string Object::toString() const
{
        std::string text = "Object: ";
        text += "id=" + ::toString(m_objectId);
        text += ", flags=" + ::toString(m_flags);
        text += ", objAge=" + ::toString(m_objectAge);
        text += ", hiddenStatusAge=" + ::toString(m_hiddenStatusAge);
        text += ", timestamp=" + m_timestamp.toString();
        text += std::string(", classification=") + objectClassificationToString(m_classification);

        text += ", classificationAge=" + ::toString(m_classificationAge);

        text += ", classificationQuality=" + ::toString(m_classificationQuality, 2);

        text += ", centerPoint=" + m_centerPoint.toString();
        text += ", centerPointSigma=" + m_centerPointSigma.toString();

        text += ", courseAngle=" + ::toString(m_courseAngle, 2);
        text += ", courseAngleSigma=" + ::toString(m_courseAngleSigma, 2);

        text += ", relativeVelocity=" + m_relativeVelocity.toString();
        text += ", relativeVelocitySigma=" + m_relativeVelocitySigma.toString();

        text += ", absoluteVelocity=" + m_absoluteVelocity.toString();
        text += ", absoluteVelocitySigma=" + m_absoluteVelocitySigma.toString();

        text += ", objectBox=" + m_objectBox.toString();
        text += ", objectBoxSigma=" + m_objectBoxSigma.toString();

        text += ", boundingBox=" + m_boundingBox.toString();
        text += ", closestPoint=" + m_closestPoint.toString();

        text += ", contourPointsNum=" + ::toString(m_contourPoints.size());
        text += ", contourPoints=" + Polygon2D(m_contourPoints).toString();

        text += ", vehicleWLANid=" + ::toString(m_vehicleWLANid, 2);
        text += ", objectHeight=" + ::toString(m_objectHeight, 2);
        text += ", objectHeightSigma=" + ::toString(m_objectHeightSigma, 2);
        text += ", objectMass=" + ::toString(m_objectMass, 2);

        text += ", maxAbsoluteVelocity=" + ::toString(m_maxAbsoluteVelocity, 2);
        text += ", normalizedMeanPointDist=" + ::toString(m_normalizedMeanPointDist, 2);
        text += ", totalTrackingDuration=" + ::toString(m_totalTrackingDuration, 2);
        text += ", totalTrackedPathLength=" + ::toString(m_totalTrackedPathLength, 2);

        return text;
}

#define TYPE_TO_STR(tstr) tstr : return #tstr

const char* Object::objectClassificationToString(ObjectClassification v)
{
        switch (v)
        {
        case TYPE_TO_STR ( Unclassified );
        case TYPE_TO_STR ( UnknownSmall );
        case TYPE_TO_STR ( UnknownBig );
        case TYPE_TO_STR ( Pedestrian );
        case TYPE_TO_STR ( Bike );
        case TYPE_TO_STR ( Car );
        case TYPE_TO_STR ( Truck );
        case TYPE_TO_STR ( Structure_Pylon );
        case TYPE_TO_STR ( Structure_Beacon );
        case TYPE_TO_STR ( Structure_GuardRail );
        case TYPE_TO_STR ( Structure_ConcreteBarrier );
        case TYPE_TO_STR ( NumClasses );
        case TYPE_TO_STR ( Unknown );
        default:
                throw std::invalid_argument("Unknown object classification " + ::toString(int(v)));
                return "<unknown>";
        }
}

Object::ObjectClassification Object::stringToObjectClassification(const std::string& s)
{
        if (s == "Unclassified" || s == "Unclass")
                return Unclassified;
        else if (s == "UnknownSmall" || s == "us")
                return UnknownSmall;
        else if (s == "UnknownBig" || s == "UB")
                return UnknownBig;
        else if (s == "Pedestrian" || s == "Ped")
                return Pedestrian;
        else if (s == "Bike")
                return Bike;
        else if (s == "Car")
                return Car;
        else if (s == "Truck")
                return Truck;
        else if (s == "Structure_Pylon" || s == "Pylon")
                return Structure_Pylon;
        else if (s == "Structure_Beacon" || s == "Bcn")
                return Structure_Beacon;
        else if (s == "Structure_GuardRail" || s == "GrdRl")
                return Structure_GuardRail;
        else if (s == "Structure_ConcreteBarrier" || s == "CBar")
                return Structure_ConcreteBarrier;
        else if (s == "NumClasses" || s == "#Classes")
                return NumClasses;
        else if (s == "Unknown")
                return Unknown;
        else
        {
                throw std::invalid_argument("Unknown object classification string \"" + s + "\"");
                return Unknown;
        }
}

std::string Object::objectClassificationToStringWithNum(ObjectClassification v)
{
        const std::string s = objectClassificationToString(v);
        return s + " (" + ::toString((int)v) + ")";
}

const char* Object::objectClassificationToShortString(ObjectClassification v)
{
        switch (v)
        {
        case Unclassified:
                return "Unclass";
        case UnknownSmall:
                return "us";
        case UnknownBig:
                return "UB";
        case Pedestrian:
                return "Ped";
        case Bike:
                return "Bike";
        case Car:
                return "Car";
        case Truck:
                return "Truck";
        case Structure_Pylon:
                return "Pylon";
        case Structure_Beacon:
                return "Bcn";
        case Structure_GuardRail:
                return "GrdRl";
        case Structure_ConcreteBarrier:
                return "CBar";
        case NumClasses:
                return "#Classes";
        case Unknown:
                return "Unknown";
        default:
                throw std::invalid_argument("Unknown object classification " + ::toString(int(v)));
                return "<unknown>";
        }
}

// ////////////////////////////////////////////////////////////

void Object::getObjectBoxVarCovar(double &var_x, double &var_y, double &covar_xy) const
{
        // Square the stored standard deviation to get the variances
        double var_x_obj = sqr(getObjectBoxSigma().getX());
        double var_y_obj = sqr(getObjectBoxSigma().getY());

        // Rotate the variances back by the angle given by the courseAngle
        double dCos = std::cos(getCourseAngle());
        double dSin = std::sin(getCourseAngle());
        double cos_sq = sqr(dCos);
        double sin_sq = sqr(dSin);
        double cos_sin = dCos * dSin;

        // And rotate the covariance matrix C_o: To rotate C_o by the
        // rotation matrix R, we need to calculate C_v = R * C_o * R^T
        var_x    =   cos_sq * var_x_obj +  sin_sq * var_y_obj;
        covar_xy = -cos_sin * var_x_obj + cos_sin * var_y_obj;
        var_y    =   sin_sq * var_x_obj +  cos_sq * var_y_obj;
}

/*
void Object::compensateEgoMovement(const Point2D& deltaPos, double deltaAngle)
{
        m_centerPoint -= deltaPos;
        m_centerPoint = m_centerPoint.rotated(-deltaAngle);

        m_centerPointSigma = m_centerPointSigma.rotated(-deltaAngle);

        m_courseAngle -= deltaAngle;
        m_courseAngleSigma -= deltaAngle;

        m_relativeVelocity = m_relativeVelocity.rotated(-deltaAngle);
        m_relativeVelocitySigma = m_relativeVelocitySigma.rotated(-deltaAngle);
        m_absoluteVelocity = m_absoluteVelocity.rotated(-deltaAngle);
        m_absoluteVelocitySigma = m_absoluteVelocitySigma.rotated(-deltaAngle);

        m_boundingBoxCenter -= deltaPos;
        m_boundingBoxCenter = m_boundingBoxCenter.rotated(-deltaAngle);

        // m_boundingBox TODO: anpassen
        m_closestPoint -= deltaPos;
        m_closestPoint = m_closestPoint.rotated(-deltaAngle);

        // transformation of the contour points - otherwise the contour points are invalid!

        Point2DVector::iterator pt;
        for (pt = m_contourPoints.begin(); pt != m_contourPoints.end(); pt++)
        {
                *pt -= deltaPos;
                *pt = pt->rotated(-deltaAngle);
        }
}
*/

void Object::incrementObjectAge()
{
        m_objectAge++;
}

void Object::setMaxAbsoluteVelocity(double v)
{
//      if (v < 0)
//              throw InvalidArgumentException("setMaxAbsoluteVelocity called with negative argument " + ::toString(v, 2) + ", but must be non-negative.");
        m_maxAbsoluteVelocity = v;
}
void Object::setNormalizedMeanPointDist(double v)
{
//      if (v < 0)
//              throw InvalidArgumentException("setNormalizedMeanPointDist called with negative argument " + ::toString(v, 2) + ", but must be non-negative.");
        m_normalizedMeanPointDist = v;
}
void Object::setTotalTrackingDuration(double v)
{
//      if (v < 0)
//              throw InvalidArgumentException ("setTotalTrackingDuration called with negative argument " + ::toString(v, 2) + ", but must be non-negative.");
        m_totalTrackingDuration = v;
}
void Object::setTotalTrackedPathLength(double v)
{
//      if (v < 0)
//              throw InvalidArgumentException("setTotalTrackedPathLength called with negative argument " + ::toString(v,2) + ", but must be non-negative.");
        m_totalTrackedPathLength = v;
}

double Object::getMeanAbsoluteVelocity() const
{
        // mean velocity
        return (m_totalTrackingDuration > 0)
                   ? (m_totalTrackedPathLength / m_totalTrackingDuration)
                   : 0.0f;
}

//
// ******************* ObjectList ***************************
//

ObjectList::ObjectList()
        : base_class()
{
        m_datatype = Datatype_Objects;
        m_timestamp.set(0.0);
}

bool ObjectList::operator==(const ObjectList& other) const
{
        return (m_timestamp == other.m_timestamp)
                   && (static_cast<const base_class&>(*this)
                           == static_cast<const base_class&>(other));
}


void ObjectList::setTimestamp(const Time& timestamp)
{
        m_timestamp = timestamp;
}

/*
void ObjectList::compensateEgoMovement(const Point2D& deltaPos, double deltaAngle)
{
        iterator obj;
        for (obj = begin(); obj != end(); obj++)
        {
                obj->compensateEgoMovement(deltaPos, deltaAngle);
        }
}
*/

//
// Alter aller Objekte um 1 erhoehen.
//
void ObjectList::incrementObjectAge()
{
        iterator obj;
        for (obj = begin(); obj != end(); obj++)
        {
                obj->incrementObjectAge();
        }
}

}       // namespace datatypes