struct_calibration.hpp

// -*- c++ -*-
/*
 * Copyright 2022-present ifm electronic, gmbh
 * SPDX-License-Identifier: Apache-2.0
 */
 
#ifndef IFM3D_DESERIALIZE_STRUCT_CALIBRATION_HPP
#define IFM3D_DESERIALIZE_STRUCT_CALIBRATION_HPP
 
#include <array>
#include <ifm3d/deserialize/deserialize_utils.hpp>
#include <ifm3d/fg/organizer_utils.h>
#include <type_traits>
 
namespace ifm3d::calibration
{
  struct ExtrinsicOpticToUser
  {
    using Ptr = std::shared_ptr<struct ExtrinsicOpticToUser>;
    float trans_x; // value in meter
    float trans_y; // value in meter
    float trans_z; // value in meter
    float rot_x;   // value in radians
    float rot_y;   // value in radians
    float rot_z;   // value in radians
 
    void
    Read(const uint8_t* data)
    {
      trans_x = mkval<float>(data + (sizeof(float) * 0));
      trans_y = mkval<float>(data + (sizeof(float) * 1));
      trans_z = mkval<float>(data + (sizeof(float) * 2));
      rot_x = mkval<float>(data + (sizeof(float) * 3));
      rot_y = mkval<float>(data + (sizeof(float) * 4));
      rot_z = mkval<float>(data + (sizeof(float) * 5));
    }
  };
 
  struct Calibration
  {
    using Ptr = std::shared_ptr<struct Calibration>;
    uint32_t model_id;
    std::array<float, 32> model_parameters;
    void
    Read(const uint8_t* data)
    {
      model_id = mkval<uint32_t>(data);
      mkarray<float, 32>(data + sizeof(uint32_t), model_parameters);
    }
  };
  /*@brief Intrisnsic parameter model for the device/head*/
  using IntrinsicCalibration = struct Calibration;
  /*@brief Inverse intrisnsic parameter model for the device/head*/
  using InverseIntrinsicCalibration = struct Calibration;
 
  struct IMUSample
  {
    uint16_t hw_timestamp;
    uint64_t timestamp;
    float temperature;
    float acc_x;
    float acc_y;
    float acc_z;
    float gyro_x;
    float gyro_y;
    float gyro_z;
 
    void
    Read(const uint8_t* data)
    {
      size_t offset = 0;
 
      auto read = [&](auto& val) {
        using T = std::remove_reference_t<decltype(val)>;
        val = mkval<T>(data + offset);
        offset += sizeof(T);
      };
 
      read(hw_timestamp);
      read(timestamp);
      read(temperature);
      read(acc_x);
      read(acc_y);
      read(acc_z);
      read(gyro_x);
      read(gyro_y);
      read(gyro_z);
    }
  };
 
  /*@brief Extrinsic calibration for converting between IMU and User
   * coordinates*/
  using AlgoExtrinsicCalibration = struct ExtrinsicOpticToUser;
 
} // end namespace calibration
 
// end namespace ifm3d
 
#endif // IFM3D_DESERIALIZE_STRUCT_CALIBRATION_HPP