sensordata.h

/*******************************************************************************
 * Copyright (c) 2023 Allied Vision Technologies GmbH
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *******************************************************************************/
 
#ifndef VISIONTRANSFER_SENSORDATA_H
#define VISIONTRANSFER_SENSORDATA_H
 
#define _USE_MATH_DEFINES
#include <cmath>
 
#ifndef M_PI
#define M_PI 3.14159265358979323846 
#endif
 
namespace visiontransfer {
 
class SensorRecord {
protected:
    int timestampSec;
    int timestampUSec;
    unsigned char status;
public:
    SensorRecord(int timestampSec, int timestampUSec, unsigned char status): timestampSec(timestampSec), timestampUSec(timestampUSec), status(status) {}
    void getTimestamp(int& s, int& us) const { s = timestampSec; us = timestampUSec; }
    unsigned char getStatus() const { return status; }
};
 
class TimestampedScalar: public SensorRecord {
public:
    double value() const { return valueIntl; }
    TimestampedScalar(int timestampSec, int timestampUSec, unsigned char status, double value): SensorRecord(timestampSec, timestampUSec, status), valueIntl(value) {}
    TimestampedScalar(): SensorRecord(0, 0, 0), valueIntl(0) { }
private:
    double valueIntl;
};
 
 
class TimestampedVector: public SensorRecord {
public:
    double x() const { return xIntl; }
    double y() const { return yIntl; }
    double z() const { return zIntl; }
    TimestampedVector(int timestampSec, int timestampUSec, unsigned char status, double x, double y, double z): SensorRecord(timestampSec, timestampUSec, status), xIntl(x), yIntl(y), zIntl(z) {}
    TimestampedVector(): SensorRecord(0, 0, 0), xIntl(0), yIntl(0), zIntl(0) { }
private:
    double xIntl, yIntl, zIntl;
};
 
class TimestampedQuaternion: public SensorRecord {
public:
    double x() const { return xIntl; }
    double y() const { return yIntl; }
    double z() const { return zIntl; }
    double w() const { return wIntl; }
    void getRollPitchYaw(double& roll, double& pitch, double& yaw) {
        // roll
        double sinr_cosp = 2 * (wIntl * xIntl + -zIntl * yIntl);
        double cosr_cosp = 1 - 2 * (xIntl * xIntl + -zIntl * -zIntl);
        roll = std::atan2(sinr_cosp, cosr_cosp);
        // pitch
        double sinp = 2 * (wIntl * -zIntl - yIntl * xIntl);
        pitch = (std::abs(sinp) >= 1) ? ((sinp<0)?-(M_PI/2):(M_PI/2)) : std::asin(sinp);
        // yaw
        double siny_cosp = 2 * (wIntl * yIntl + xIntl * -zIntl);
        double cosy_cosp = 1 - 2 * (-zIntl * -zIntl + yIntl * yIntl);
        yaw = std::atan2(siny_cosp, cosy_cosp);
    }
    double accuracy() const { return accuracyIntl; }
    TimestampedQuaternion(int timestampSec, int timestampUSec, unsigned char status, double x, double y, double z, double w, double accuracy): SensorRecord(timestampSec, timestampUSec, status), xIntl(x), yIntl(y), zIntl(z), wIntl(w), accuracyIntl(accuracy) {}
    TimestampedQuaternion(): SensorRecord(0, 0, 0), xIntl(0), yIntl(0), zIntl(0), wIntl(0), accuracyIntl(0) { }
private:
    double xIntl, yIntl, zIntl, wIntl;
    double accuracyIntl;
};
 
} // namespace
 
#endif