116 lines
4.5 KiB
C++
116 lines
4.5 KiB
C++
/*******************************************************************************
|
|
* 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
|
|
|