1#ifndef __DVSENSE_CALIBRATOR_HPP__
2#define __DVSENSE_CALIBRATOR_HPP__
5#define DVSENSE_API __declspec(dllexport)
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"
20#define DVSENSE_API __declspec(dllexport)
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;
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);
51 std::lock_guard<std::mutex> lock(aps_to_dvs_lookup_mutex_);
57 rebuildApsToDvsLookup(0.0, 0.0);
67 std::lock_guard<std::mutex> lock(aps_to_dvs_lookup_mutex_);
68 for(
auto &ev = begin; ev != end; ++ev)
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]);
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)
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_)
112 rebuildApsToDvsLookup(offset_x, offset_y);
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_)
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];
127 dst[dst_offset + 1] = 0;
128 dst[dst_offset + 2] = 0;
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_;
148 param_.affine_matrix[
"0"].data = aps_to_dvs_affine_matrix_;
149 paramToJsonFile(param_, file_path);
152 rebuildApsToDvsLookup(0.0, 0.0);
157 double a = src[0], b = src[1], tx = src[2];
158 double c = src[3], d = src[4], ty = src[5];
160 double det = a * d - b * c;
161 if (fabs(det) < 1e-12)
166 double inv_det = 1.0 / det;
168 double a_ = d * inv_det;
169 double b_ = -b * inv_det;
170 double c_ = -c * inv_det;
171 double d_ = a * inv_det;
173 double tx_ = -(a_ * tx + b_ * ty);
174 double ty_ = -(c_ * tx + d_ * ty);
185 void rebuildApsToDvsLookup(
double offset_x,
double offset_y)
187 aps_to_dvs_source_byte_offsets_.resize(param_.dvs_cols * param_.dvs_rows);
188 for(
int y = 0; y < param_.dvs_rows; ++y)
190 for(
int x = 0; x < param_.dvs_cols; ++x)
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;
199 cached_offset_x_ = offset_x;
200 cached_offset_y_ = offset_y;
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_;
Contiguous BGR888 APS image with exposure timestamps.
Definition ApsTypes.h:34
TimeStamp exposure_end_timestamp
Definition ApsTypes.h:84
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 ¶m)
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
std::map< std::string, MatrixData > affine_matrix
Definition TypeUtils.hpp:45
Event2D * EventIterator_t
Definition EventTypes.hpp:63
Definition TypeUtils.hpp:35