demo version

demo/camera_client/dependencies/libvisiontransfer/visiontransfer/sensordata.h

@@ -0,0 +1,116 @@
+/*******************************************************************************
+ * Copyright (c) 2021 Nerian Vision 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 {
+
+/**
+ * \brief Base class for sensor records with timestamp and status (reliability) fields
+ */
+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) {}
+    /**
+     * Returns the sensor-reported timestamp for the reading
+     */
+    void getTimestamp(int& s, int& us) const { s = timestampSec; us = timestampUSec; }
+    /**
+     * Returns the current sensor calibration status (range 0..3)
+     * 0: Sensor unreliable; 1: Accuracy low; 2: ~ medium; 3: ~ high
+     */
+    unsigned char getStatus() const { return status; }
+};
+
+/**
+ * \brief Encapsulate a scalar sensor measurement, containing the value, as well as timestamp and status fields
+ */
+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;
+};
+
+
+/**
+ * \brief Encapsulate a 3D sensor report, containing X, Y, Z, as well as timestamp and status fields
+ */
+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;
+};
+
+/**
+ * \brief Encapsulate a 4D (quaternion) sensor report, containing X, Y, Z, W, as well as timestamp and status fields and measurement accuracy
+ *
+ * Component r is the real part of the quaternion, also called w (ijkr corresponds to xyzw).
+ */
+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; }
+    /**
+     * Convert the quaternion to device roll, pitch, and yaw (radians)
+     */
+    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);
+    }
+    /**
+     * Returns the internal device-reported angular accuracy (radians)
+     */
+    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
+