system_elements.hpp

// Copyright (C) 2023 Alessandro Fornasier.
// Control of Networked Systems, University of Klagenfurt, Austria.
//
// All rights reserved.
//
// This software is licensed under the terms of the BSD-2-Clause-License with
// no commercial use allowed, the full terms of which are made available
// in the LICENSE file. No license in patents is granted.
//
// You can contact the authors at <alessandro.fornasier@ieee.org>
 
#ifndef SYSTEM_ELEMENTS_HPP
#define SYSTEM_ELEMENTS_HPP
 
#include <memory>
 
#include "types/fptypes.hpp"
 
namespace msceqf
{
enum class SystemStateElementName
{
  T,  
  b,  
  S,  
  K,  
};
 
class SystemStateElement
{
 public:
  virtual ~SystemStateElement() = default;
 
  virtual std::unique_ptr<SystemStateElement> clone() const = 0;
 
 protected:
  SystemStateElement() = default;
  SystemStateElement(const SystemStateElement&) = default;
  SystemStateElement(SystemStateElement&&) = default;
  SystemStateElement& operator=(const SystemStateElement&) = default;
  SystemStateElement& operator=(SystemStateElement&&) = default;
};
class SystemStateElement {…};
 
struct ExtendedPoseState final : public SystemStateElement
{
  ExtendedPoseState() : T_(){};
  ExtendedPoseState(const SE23& T) : T_(T){};
 
  std::unique_ptr<SystemStateElement> clone() const override { return std::make_unique<ExtendedPoseState>(*this); }
 
  SE23 T_;  
};
struct ExtendedPoseState final : public SystemStateElement {…};
 
struct BiasState final : public SystemStateElement
{
  BiasState() : b_(Vector6::Zero()){};
  BiasState(const Vector6& b) : b_(b){};
 
  std::unique_ptr<SystemStateElement> clone() const override { return std::make_unique<BiasState>(*this); }
 
  Vector6 b_;  
};
struct BiasState final : public SystemStateElement {…};
 
struct CameraExtrinsicState final : public SystemStateElement
{
  CameraExtrinsicState() : S_(){};
  CameraExtrinsicState(const Quaternion& q, const Vector3& t) : S_(q, {t}){};
  CameraExtrinsicState(const Matrix3& R, const Vector3& t) : S_(R, {t}){};
  CameraExtrinsicState(const Matrix4& S) : S_(S){};
  CameraExtrinsicState(const SE3& S) : S_(S){};
 
  std::unique_ptr<SystemStateElement> clone() const override { return std::make_unique<CameraExtrinsicState>(*this); }
 
  SE3 S_;  
};
struct CameraExtrinsicState final : public SystemStateElement {…};
 
struct CameraIntrinsicState final : public SystemStateElement
{
  CameraIntrinsicState() : K_(){};
  CameraIntrinsicState(const fp& fx, const fp& fy, const fp& cx, const fp& cy) : K_(fx, fy, cx, cy){};
  CameraIntrinsicState(const Vector4& intr) : K_(intr){};
  CameraIntrinsicState(const Matrix3& K) : K_(K){};
  CameraIntrinsicState(const In& K) : K_(K){};
 
  std::unique_ptr<SystemStateElement> clone() const override { return std::make_unique<CameraIntrinsicState>(*this); }
 
  In K_;  
};
struct CameraIntrinsicState final : public SystemStateElement {…};
 
struct FeatureState final : public SystemStateElement
{
 public:
  FeatureState() : f_(Vector3::Zero()){};
  FeatureState(const Vector3& f) : f_(f){};
  FeatureState(const fp& x, const fp& y, const fp& z) : f_({x, y, z}){};
 
  std::unique_ptr<SystemStateElement> clone() const override { return std::make_unique<FeatureState>(*this); }
 
  Vector3 f_;  
};
struct FeatureState final : public SystemStateElement {…};
 
using SystemStateElementSharedPtr = std::shared_ptr<SystemStateElement>;
using SystemStateElementUniquePtr = std::unique_ptr<SystemStateElement>;
using ExtendedPoseStateSharedPtr = std::shared_ptr<ExtendedPoseState>;
using ExtendedPoseUniquePtr = std::unique_ptr<ExtendedPoseState>;
using BiasStateStateSharedPtr = std::shared_ptr<BiasState>;
using BiasStateStateUniquePtr = std::unique_ptr<BiasState>;
using CameraExtrinsicStateStateSharedPtr = std::shared_ptr<CameraExtrinsicState>;
using CameraExtrinsicStateStateUniquePtr = std::unique_ptr<CameraExtrinsicState>;
using CameraIntrinsicStateStateSharedPtr = std::shared_ptr<CameraIntrinsicState>;
using CameraIntrinsicStateStateUniquePtr = std::unique_ptr<CameraIntrinsicState>;
using FeatureStateStateStateSharedPtr = std::shared_ptr<FeatureState>;
using FeatureStateStateStateUniquePtr = std::unique_ptr<FeatureState>;
 
template <typename T, typename... Args>
[[nodiscard]] static SystemStateElementUniquePtr createSystemStateElement(const std::tuple<Args...>& args)
{
  return std::apply(
      [](const auto&... args) {
        if constexpr (std::is_base_of_v<SystemStateElement, T> && std::is_constructible_v<T, decltype(args)...>)
        {
          return std::make_unique<T>(args...);
        }
        else
        {
          return nullptr;
        }
      },
      args);
}
 
}  // namespace msceqf
 
#endif  // SYSTEM_ELEMENTS_HPP