libvisiontransfer  8.3.0
sensordata.h
1 /*******************************************************************************
2  * Copyright (c) 2020 Nerian Vision GmbH
3  *
4  * Permission is hereby granted, free of charge, to any person obtaining a copy
5  * of this software and associated documentation files (the "Software"), to deal
6  * in the Software without restriction, including without limitation the rights
7  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
8  * copies of the Software, and to permit persons to whom the Software is
9  * furnished to do so, subject to the following conditions:
10  *
11  * The above copyright notice and this permission notice shall be included in
12  * all copies or substantial portions of the Software.
13  *******************************************************************************/
14 
15 #ifndef VISIONTRANSFER_SENSORDATA_H
16 #define VISIONTRANSFER_SENSORDATA_H
17 
18 #define _USE_MATH_DEFINES
19 #include <cmath>
20 
21 #ifndef M_PI
22 #define M_PI 3.14159265358979323846
23 #endif
24 
25 namespace visiontransfer {
26 
30 class SensorRecord {
31 protected:
32  int timestampSec;
33  int timestampUSec;
34  unsigned char status;
35 public:
36  SensorRecord(int timestampSec, int timestampUSec, unsigned char status): timestampSec(timestampSec), timestampUSec(timestampUSec), status(status) {}
40  void getTimestamp(int& s, int& us) const { s = timestampSec; us = timestampUSec; }
45  unsigned char getStatus() const { return status; }
46 };
47 
52 public:
53  double value() const { return valueIntl; }
54  TimestampedScalar(int timestampSec, int timestampUSec, unsigned char status, double value): SensorRecord(timestampSec, timestampUSec, status), valueIntl(value) {}
55  TimestampedScalar(): SensorRecord(0, 0, 0), valueIntl(0) { }
56 private:
57  double valueIntl;
58 };
59 
60 
65 public:
66  double x() const { return xIntl; }
67  double y() const { return yIntl; }
68  double z() const { return zIntl; }
69  TimestampedVector(int timestampSec, int timestampUSec, unsigned char status, double x, double y, double z): SensorRecord(timestampSec, timestampUSec, status), xIntl(x), yIntl(y), zIntl(z) {}
70  TimestampedVector(): SensorRecord(0, 0, 0), xIntl(0), yIntl(0), zIntl(0) { }
71 private:
72  double xIntl, yIntl, zIntl;
73 };
74 
81 public:
82  double x() const { return xIntl; }
83  double y() const { return yIntl; }
84  double z() const { return zIntl; }
85  double w() const { return wIntl; }
89  void getRollPitchYaw(double& roll, double& pitch, double& yaw) {
90  // roll
91  double sinr_cosp = 2 * (wIntl * xIntl + -zIntl * yIntl);
92  double cosr_cosp = 1 - 2 * (xIntl * xIntl + -zIntl * -zIntl);
93  roll = std::atan2(sinr_cosp, cosr_cosp);
94  // pitch
95  double sinp = 2 * (wIntl * -zIntl - yIntl * xIntl);
96  pitch = (std::abs(sinp) >= 1) ? ((sinp<0)?-(M_PI/2):(M_PI/2)) : std::asin(sinp);
97  // yaw
98  double siny_cosp = 2 * (wIntl * yIntl + xIntl * -zIntl);
99  double cosy_cosp = 1 - 2 * (-zIntl * -zIntl + yIntl * yIntl);
100  yaw = std::atan2(siny_cosp, cosy_cosp);
101  }
105  double accuracy() const { return accuracyIntl; }
106  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) {}
107  TimestampedQuaternion(): SensorRecord(0, 0, 0), xIntl(0), yIntl(0), zIntl(0), wIntl(0), accuracyIntl(0) { }
108 private:
109  double xIntl, yIntl, zIntl, wIntl;
110  double accuracyIntl;
111 };
112 
113 } // namespace
114 
115 #endif
116 
visiontransfer::SensorRecord::getStatus
unsigned char getStatus() const
Definition: sensordata.h:45
visiontransfer::TimestampedQuaternion
Encapsulate a 4D (quaternion) sensor report, containing X, Y, Z, W, as well as timestamp and status f...
Definition: sensordata.h:80
visiontransfer::SensorRecord::getTimestamp
void getTimestamp(int &s, int &us) const
Definition: sensordata.h:40
visiontransfer::SensorRecord
Base class for sensor records with timestamp and status (reliability) fields.
Definition: sensordata.h:30
visiontransfer::TimestampedQuaternion::accuracy
double accuracy() const
Definition: sensordata.h:105
visiontransfer::TimestampedVector
Encapsulate a 3D sensor report, containing X, Y, Z, as well as timestamp and status fields.
Definition: sensordata.h:64
visiontransfer::TimestampedScalar
Encapsulate a scalar sensor measurement, containing the value, as well as timestamp and status fields...
Definition: sensordata.h:51
visiontransfer::TimestampedQuaternion::getRollPitchYaw
void getRollPitchYaw(double &roll, double &pitch, double &yaw)
Definition: sensordata.h:89
Nerian Vision Technologies