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 Apache License, Version 2.0
// (the "License"); you may not use this file except in compliance with the
// License. You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
// License for the specific language governing permissions and limitations
// under the License.
//
// 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;
};

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 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 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 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 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_;  
};
 
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