1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
|
#include "profile.h"
#include <iostream>
#include <QDebug>
#include <QJsonArray>
Profile::Profile(
const Pixels& pixels,
const CalibrationTablePtr calibrationTableZ,
const CalibrationTablePtr calibrationTableX)
: m_counters(pixels.counters)
{
if (!calibrationTableZ || !calibrationTableX)
{
std::cerr << __func__ << ": got invalid calibration tables"
<< std::endl;
return;
}
static bool done{false};
if (!done) {
for (size_t i = 9471; i < 9472; i++) {
std::cout << "row #" << i << ": ";
for (size_t j = 0; j < 1280; ++j) {
const auto& x = calibrationTableX->at(j).at(i);
const auto& z = calibrationTableZ->at(j).at(i);
std::cout << "Profile: table: " << x << ' ' << z << std::endl;
}
std::cout << std::endl;
}
}
for (size_t i = 0; i < img_width; ++i) {
try {
const auto& pixel = pixels.pixels[i];
const auto pixelDiscrete = pixel * discretesInRage / img_height;
if (Q_UNLIKELY(pixel >= sizeof(CalibrationColumn) / sizeof(float))) {
qWarning() << "got invalid calibration pixel at" << i << ":"
<< pixel;
m_counters = {};
m_pointsMm = {QPointF{std::nan(""), std::nan("")}};
return;
}
if (Q_UNLIKELY(pixel == sizeof(CalibrationColumn) - 1)) {
qDebug() << "Profile: got edge value";
const auto& z = calibrationTableZ->at(i).at(pixelDiscrete);
if (qFuzzyIsNull(z) || std::isnan(z)) {
m_pointsMm.at(i) = {std::nan(""), std::nan("")};
continue;
}
const auto& x = calibrationTableX->at(i).at(pixelDiscrete);
m_pointsMm.at(i) = {x, z};
continue;
}
const auto& leftMmZ = calibrationTableZ->at(i).at(
size_t(pixelDiscrete));
const auto& rightMmZ = calibrationTableZ->at(i).at(
size_t(pixelDiscrete) + 1);
const auto& leftMmX = calibrationTableX->at(i).at(
size_t(pixelDiscrete));
const auto& rightMmX = calibrationTableX->at(i).at(
size_t(pixelDiscrete) + 1);
const auto& fract = pixelDiscrete - long(pixelDiscrete);
const auto z = (leftMmZ * (1 - fract) + rightMmZ * fract);
// TODO: use only NaN (or zero?) everywhere
// NOTE: QJsonValue converts NaN to zero
if (qFuzzyIsNull(z) || std::isnan(z)) {
// qDebug() << "got nan z for discrete" << pixelDiscrete << leftMmZ
// << rightMmZ;
m_pointsMm.at(i) = {std::nan(""), std::nan("")};
continue;
}
const auto x = (leftMmX * (1 - fract) + rightMmX * fract);
m_pointsMm.at(i) = {x, z};
if (!done) {
qDebug() << "got these values\t" << pixels.pixels[i]
<< pixelDiscrete << m_pointsMm[i] << leftMmZ
<< rightMmZ << leftMmX << rightMmX;
}
} catch (const std::out_of_range& ex) {
qWarning() << "out of range exception:" << ex.what();
qWarning() << "i:" << i; // << "pixels[i]" << pixels.pixels[i];
m_counters = {};
m_pointsMm = {QPointF{std::nan(""), std::nan("")}};
return;
} catch (const std::exception& ex) {
qWarning() << "exception:" << ex.what();
qWarning() << "i:" << i << "pixels[i]" << pixels.pixels[i];
m_counters = {};
m_pointsMm = {QPointF{std::nan(""), std::nan("")}};
return;
} catch (...) {
qWarning() << "unknown exception";
m_counters = {};
m_pointsMm = {QPointF{std::nan(""), std::nan("")}};
return;
}
}
done = true;
// static bool printOnce = [&]() -> bool {
// for (size_t i = 0; i < m_pointsMm.size(); ++i) {
// qDebug() << '\t' << pixels.pixels[i] << m_pointsMm[i];
// }
// return true;
// }();
}
const Counters& Profile::counters() const
{
return m_counters;
}
const Profile::PointsMm& Profile::pointsMm() const
{
return m_pointsMm;
}
Profile::operator const QJsonObject() const
{
QJsonObject counters{
{"timestampUs", qint64(m_counters.timestampUs)},
{"measurementCounter", qint64(m_counters.measurementCounter)},
{"encoderPosition", qint64(m_counters.encoderPosition)},
};
QJsonArray jsonPoints;
for (const auto& p : m_pointsMm) {
jsonPoints << QJsonArray{p.x(), p.y()};
}
return {{"counters", counters}, {"pointsMm", jsonPoints}};
}
|
