ifm3d
struct_tof_info_v3.hpp
1 // -*- c++ -*-
2 /*
3  * Copyright 2022-present ifm electronic, gmbh
4  * SPDX-License-Identifier: Apache-2.0
5  */
6 
7 #ifndef IFM3D_DESERIALIZE_STRUCT_TOF_INFO_V3_HPP
8 #define IFM3D_DESERIALIZE_STRUCT_TOF_INFO_V3_HPP
9 
10 #include <array>
11 #include <string>
12 #include <ifm3d/device/device.h>
13 #include <ifm3d/device/err.h>
14 #include <ifm3d/fg/organizer_utils.h>
15 #include <ifm3d/fg/buffer.h>
16 #include <ifm3d/deserialize/deserialize_utils.hpp>
17 #include <ifm3d/deserialize/struct_calibration.hpp>
18 
19 namespace ifm3d
20 {
21  namespace
22  {
23  constexpr auto TOF_INFO_VERSION_INDEX = 0x0000;
24  constexpr auto TOF_INFO_DISTANCE_RESOLUTION_INDEX = 0x0004;
25  constexpr auto TOF_INFO_AMPLITUDE_RESOLUTION_INDEX = 0x0008;
26  constexpr auto TOF_INFO_AMPLITUDE_NORM_FACTOR_INDEX = 0x000C;
27  constexpr auto TOF_INFO_EXTRINSIC_OPTICAL_TO_USER_INDEX = 0x0018;
28  constexpr auto TOF_INFO_INTRINSIC_CALIBRATION_INDEX = 0x0030;
29  constexpr auto TOF_INFO_INVERSE_INTRINSIC_CALIBRATION_INDEX = 0x00B4;
30  constexpr auto TOF_INFO_EXPOSURE_TIMESTAMPS_INDEX = 0x0138;
31  constexpr auto TOF_INFO_EXPOSURE_TIMES_INDEX = 0x0150;
32  constexpr auto TOF_INFO_ILLUTEMPERATURE_INDEX = 0x015C;
33  constexpr auto TOF_INFO_MODE_INDEX = 0x0160;
34  constexpr auto TOF_INFO_IMAGER_INDEX = 0x0180;
35  };
36 
38  class TOFInfoV3
39  {
40 
41  public:
42  using Ptr = std::shared_ptr<TOFInfoV3>;
43 
44  static bool
45  IsValid(const uint8_t* data, size_t size)
46  {
47  uint32_t version = mkval<std::uint32_t>(data + TOF_INFO_VERSION_INDEX);
48 
49  if (size < tof_info_v3_size || version < 3)
50  {
51  return false;
52  }
53  return true;
54  }
55 
56  void
57  Read(const uint8_t* data, size_t size)
58  {
59  if (!IsValid(data, size))
60  {
61  throw ifm3d::Error(IFM3D_CORRUPTED_STRUCT);
62  }
63  const uint8_t* start_ptr = data;
64  std::array<char, 32> buff;
65  version = mkval<std::uint32_t>(start_ptr + TOF_INFO_VERSION_INDEX);
66  distance_resolution =
67  mkval<float>(start_ptr + TOF_INFO_DISTANCE_RESOLUTION_INDEX);
68  amplitude_resolution =
69  mkval<float>(start_ptr + TOF_INFO_AMPLITUDE_RESOLUTION_INDEX);
70  mkarray<float, 3>(start_ptr + TOF_INFO_AMPLITUDE_NORM_FACTOR_INDEX,
71  amp_normalization_factors);
72  extrinsic_optic_to_user.Read(start_ptr +
73  TOF_INFO_EXTRINSIC_OPTICAL_TO_USER_INDEX);
74  intrinsic_calibration.Read(start_ptr +
75  TOF_INFO_INTRINSIC_CALIBRATION_INDEX);
76  inverse_intrinsic_calibration.Read(
77  start_ptr + TOF_INFO_INVERSE_INTRINSIC_CALIBRATION_INDEX);
78  mkarray<uint64_t, 3>(start_ptr + TOF_INFO_EXPOSURE_TIMESTAMPS_INDEX,
79  exposure_timestamps_ns);
80  mkarray<float, 3>(start_ptr + TOF_INFO_EXPOSURE_TIMES_INDEX,
81  exposure_times_s);
82  illu_temperature =
83  mkval<float>(start_ptr + TOF_INFO_ILLUTEMPERATURE_INDEX);
84  mkarray<char, 32>(start_ptr + TOF_INFO_MODE_INDEX, buff);
85  mode = std::string(buff.begin(), buff.end());
86  buff.fill('\0');
87  mkarray<char, 32>(start_ptr + TOF_INFO_IMAGER_INDEX, buff);
88  imager = std::string(buff.begin(), buff.end());
89  buff.fill('\0');
90  };
91  /*@brief Version of the TOF_INFO data */
92  uint32_t version;
93  /*@brief Resolution of distance buffer per digit[m]*/
94  float distance_resolution;
95  /*@brief Resolution of the amplitude buffer*/
96  float amplitude_resolution;
97  /*@brief Amplitude normalization factors for the individual exposure
98  * times*/
99  std::array<float, 3> amp_normalization_factors;
100  /*@brief Extrinsic optic parameter to user*/
101  calibration::ExtrinsicOpticToUser extrinsic_optic_to_user;
102  /*@brief Intrinsic calibration parameters*/
103  calibration::IntrinsicCalibration intrinsic_calibration;
104  /*@brief Inverse intrinsic calibration parameters*/
105  calibration::InverseIntrinsicCalibration inverse_intrinsic_calibration;
106  /*@brief The timestamp of the individual exposure time [nano seconds]*/
107  std::array<uint64_t, 3> exposure_timestamps_ns;
108  /*@brief Actual exposure times of a single phase image [seconds].*/
109  std::array<float, 3> exposure_times_s;
110  /*@brief Illumination temperature*/
111  float illu_temperature;
112  /*@brief Mode of the head*/
113  std::string mode;
114  /*@brief Imager type*/
115  std::string imager;
116  /*@brief TOF_INFO data size in bytes*/
117  static constexpr size_t tof_info_v3_size = 416;
118 
119  static TOFInfoV3
120  Deserialize(const Buffer& tof_info_buffer)
121  {
122  TOFInfoV3 tof_info_v3;
123 
124  tof_info_v3.Read(tof_info_buffer.ptr<uint8_t>(0),
125  tof_info_buffer.size());
126  return tof_info_v3;
127  }
128  };
129 } // end namespace ifm3d
130 
131 #endif // IFM3D_DESERIALIZE_STRUCT_TOF_INFO_V3_HPP
ifm3d::Buffer::size
size_t size() const
Return the size of the buffer in bytes.
ifm3d::Buffer
The class Buffer represent a STL container to store data from the ifm devices in 2 dimension and supp...
Definition: buffer.h:99
ifm3d::Buffer::ptr
T * ptr(const std::uint32_t row)
returns a pointer to the specified Buffer row.
ifm3d::TOFInfoV3
Definition: struct_tof_info_v3.hpp:38
ifm3d::Error
Definition: err.h:114
ifm3d::calibration::ExtrinsicOpticToUser
All items are given in SI units, i.e. transXYZ are in [m] and rotXYZ are in [rad].
Definition: struct_calibration.hpp:24
ifm3d::calibration::Calibration
Definition: struct_calibration.hpp:61