DvsenseDriver  1.3.0
The SDK for dvsense products.
Loading...
Searching...
No Matches
Calibrator.hpp
1#ifndef __DVSENSE_CALIBRATOR_HPP__
2#define __DVSENSE_CALIBRATOR_HPP__
3
4#ifdef _WIN32
5#define DVSENSE_API __declspec(dllexport)
6#else
7#define DVSENSE_API
8#endif // _WIN32
9#include <string>
10#include <atomic>
11#include <mutex>
12#include "DvsenseBase/EventBase/EventTypes.hpp"
13#include "DvsenseBase/Utils/Json/nlohmann/json.hpp"
14#include "DvsenseBase/EventBase/EventTypes.hpp"
15#include "DvsenseBase/ApsBase/ApsTypes.h"
16#include "DvsenseBase/Utils/TypeUtils.hpp"
17#include "DvsenseBase/Utils/Json/JsonUtils.hpp"
18
19#ifdef _WIN32
20#define DVSENSE_API __declspec(dllexport)
21#else
22#define DVSENSE_API
23#endif // _WIN32
24
25namespace dvsense {
26
31class DVSENSE_API Calibrator
32{
33public:
35 {
36 // Load default param
37 param_.dvs_cols = 1280;
38 param_.dvs_rows = 720;
39 param_.aps_cols = 3840;
40 param_.aps_rows = 2160;
41 static const double pixel_size_ratio = 4.86 / 1.45; // DVS pixel size ratio to APS
42 dvs_to_aps_affine_matrix_ = {pixel_size_ratio, 0, (-param_.dvs_cols / 2) * pixel_size_ratio + param_.aps_cols / 2,
43 0, pixel_size_ratio, (-param_.dvs_rows / 2) * pixel_size_ratio + param_.aps_rows / 2};
44 aps_to_dvs_affine_matrix_ = {1 / pixel_size_ratio, 0, (-param_.aps_cols / 2) * (1 / pixel_size_ratio) + param_.dvs_cols / 2,
45 0, 1 / pixel_size_ratio, (-param_.aps_rows / 2) * (1 / pixel_size_ratio) + param_.dvs_rows / 2};
46 rebuildApsToDvsLookup(0.0, 0.0);
47 }
48
50 {
51 std::lock_guard<std::mutex> lock(aps_to_dvs_lookup_mutex_);
52 param_ = param;
53 dvs_to_aps_affine_matrix_ = param.affine_matrix["1"].data;
54 aps_to_dvs_affine_matrix_ = param.affine_matrix["0"].data;
55 offset_x_ = 0.0;
56 offset_y_ = 0.0;
57 rebuildApsToDvsLookup(0.0, 0.0);
58 return true;
59 }
60
66 {
67 std::lock_guard<std::mutex> lock(aps_to_dvs_lookup_mutex_);
68 for(auto &ev = begin; ev != end; ++ev)
69 {
70 // Apply affine transformation to map DVS events to APS frame
71 int x = static_cast<int>(ev->x * dvs_to_aps_affine_matrix_[0] + ev->y * dvs_to_aps_affine_matrix_[1] + dvs_to_aps_affine_matrix_[2]);
72 int y = static_cast<int>(ev->x * dvs_to_aps_affine_matrix_[3] + ev->y * dvs_to_aps_affine_matrix_[4] + dvs_to_aps_affine_matrix_[5]);
73 ev->x = x;
74 ev->y = y;
75 }
76 }
77
79 {
80 ApsFrame dst_frame(param_.dvs_cols, param_.dvs_rows, src_frame.exposure_start_timestamp, src_frame.exposure_end_timestamp);
81 mapApsToDvsBgr(src_frame, dst_frame.data(), dst_frame.getDataSize());
82 return dst_frame;
83 }
84
99 bool mapApsToDvsBgr(ApsFrame &src_frame, uint8_t *dst, size_t dst_size)
100 {
101 std::lock_guard<std::mutex> lock(aps_to_dvs_lookup_mutex_);
102 const size_t required_size = static_cast<size_t>(param_.dvs_cols) * param_.dvs_rows * 3;
103 if(dst == nullptr || dst_size < required_size)
104 {
105 return false;
106 }
107 const double offset_x = offset_x_.load();
108 const double offset_y = offset_y_.load();
109 if (aps_to_dvs_source_byte_offsets_.size() != static_cast<size_t>(param_.dvs_cols * param_.dvs_rows) ||
110 offset_x != cached_offset_x_ || offset_y != cached_offset_y_)
111 {
112 rebuildApsToDvsLookup(offset_x, offset_y);
113 }
114 const uint8_t *src = src_frame.data();
115 size_t dst_offset = 0;
116 for(const int32_t src_offset : aps_to_dvs_source_byte_offsets_)
117 {
118 if(src_offset >= 0)
119 {
120 dst[dst_offset] = src[src_offset];
121 dst[dst_offset + 1] = src[src_offset + 1];
122 dst[dst_offset + 2] = src[src_offset + 2];
123 }
124 else
125 {
126 dst[dst_offset] = 0;
127 dst[dst_offset + 1] = 0;
128 dst[dst_offset + 2] = 0;
129 }
130 dst_offset += 3;
131 }
132 return true;
133 }
134
135 void adjustAffineTranslation(double dx, double dy)
136 {
137 offset_x_ = dx;
138 offset_y_ = dy;
139 }
140
141 void saveCalibrationParam(std::string &file_path)
142 {
143 std::lock_guard<std::mutex> lock(aps_to_dvs_lookup_mutex_);
144 dvs_to_aps_affine_matrix_[2] += offset_x_;
145 dvs_to_aps_affine_matrix_[5] += offset_y_;
146 param_.affine_matrix["1"].data = dvs_to_aps_affine_matrix_;
147 computeInverseAffine(dvs_to_aps_affine_matrix_.data(), aps_to_dvs_affine_matrix_.data());
148 param_.affine_matrix["0"].data = aps_to_dvs_affine_matrix_;
149 paramToJsonFile(param_, file_path);
150 offset_x_ = 0;
151 offset_y_ = 0;
152 rebuildApsToDvsLookup(0.0, 0.0);
153 }
154
155 void computeInverseAffine(const double src[6], double dst[6])
156 {
157 double a = src[0], b = src[1], tx = src[2];
158 double c = src[3], d = src[4], ty = src[5];
159
160 double det = a * d - b * c;
161 if (fabs(det) < 1e-12)
162 {
163 return;
164 }
165
166 double inv_det = 1.0 / det;
167
168 double a_ = d * inv_det;
169 double b_ = -b * inv_det;
170 double c_ = -c * inv_det;
171 double d_ = a * inv_det;
172
173 double tx_ = -(a_ * tx + b_ * ty);
174 double ty_ = -(c_ * tx + d_ * ty);
175
176 dst[0] = a_;
177 dst[1] = b_;
178 dst[2] = tx_;
179 dst[3] = c_;
180 dst[4] = d_;
181 dst[5] = ty_;
182 }
183
184private:
185 void rebuildApsToDvsLookup(double offset_x, double offset_y)
186 {
187 aps_to_dvs_source_byte_offsets_.resize(param_.dvs_cols * param_.dvs_rows);
188 for(int y = 0; y < param_.dvs_rows; ++y)
189 {
190 for(int x = 0; x < param_.dvs_cols; ++x)
191 {
192 const int aps_x = static_cast<int>(x * dvs_to_aps_affine_matrix_[0] + y * dvs_to_aps_affine_matrix_[1] + dvs_to_aps_affine_matrix_[2] + offset_x);
193 const int aps_y = static_cast<int>(x * dvs_to_aps_affine_matrix_[3] + y * dvs_to_aps_affine_matrix_[4] + dvs_to_aps_affine_matrix_[5] + offset_y);
194 aps_to_dvs_source_byte_offsets_[y * param_.dvs_cols + x] =
195 aps_x >= 0 && aps_x < param_.aps_cols && aps_y >= 0 && aps_y < param_.aps_rows
196 ? (aps_y * param_.aps_cols + aps_x) * 3 : -1;
197 }
198 }
199 cached_offset_x_ = offset_x;
200 cached_offset_y_ = offset_y;
201 }
202
203 CalibratorParameters param_;
204 std::vector<double> dvs_to_aps_affine_matrix_;
205 std::vector<double> aps_to_dvs_affine_matrix_;
206 std::atomic<double> offset_x_ = 0.0;
207 std::atomic<double> offset_y_ = 0.0;
208 std::vector<int32_t> aps_to_dvs_source_byte_offsets_;
209 double cached_offset_x_ = 0.0;
210 double cached_offset_y_ = 0.0;
211 std::mutex aps_to_dvs_lookup_mutex_;
212};
213
214}
215
216#endif // __DVSENSE_ALGORITHMS_CALIBRATOR_HPP__
Contiguous BGR888 APS image with exposure timestamps.
Definition ApsTypes.h:34
TimeStamp exposure_end_timestamp
Definition ApsTypes.h:84
uint8_t * data()
size_t getDataSize() const
TimeStamp exposure_start_timestamp
Definition ApsTypes.h:83
void mapDvsToAps(EventIterator_t begin, EventIterator_t end)
Transform event coordinates to APS coordinates in place over [begin, end).
Definition Calibrator.hpp:65
bool mapApsToDvsBgr(ApsFrame &src_frame, uint8_t *dst, size_t dst_size)
Map an APS image into reusable DVS-resolution BGR888 output storage.
Definition Calibrator.hpp:99
void saveCalibrationParam(std::string &file_path)
Definition Calibrator.hpp:141
bool loadCalibrationParam(CalibratorParameters &param)
Definition Calibrator.hpp:49
ApsFrame mapApsToDvs(ApsFrame &src_frame)
Definition Calibrator.hpp:78
Calibrator()
Definition Calibrator.hpp:34
void adjustAffineTranslation(double dx, double dy)
Definition Calibrator.hpp:135
void computeInverseAffine(const double src[6], double dst[6])
Definition Calibrator.hpp:155
Definition ApsTypes.h:19
std::map< std::string, MatrixData > affine_matrix
Definition TypeUtils.hpp:45
Event2D * EventIterator_t
Definition EventTypes.hpp:63
Definition TypeUtils.hpp:35