system.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_HPP
#define SYSTEM_HPP
 
#include <variant>
 
#include "msceqf/system/system_elements.hpp"
#include "msceqf/options/msceqf_options.hpp"
 
namespace msceqf
{
class SystemState
{
 public:
  using SystemStateKey = std::variant<SystemStateElementName, uint>;  
  using SystemStateMap = std::unordered_map<SystemStateKey, SystemStateElementSharedPtr>;  
  using SystemStateAlgebraMap = std::unordered_map<SystemStateKey, VectorX>;               

  SystemState() = delete;

  SystemState(const StateOptions& opts, const SE23& T0 = SE23(), const Vector6& b0 = Vector6::Zero());

  template <typename... Args>
  SystemState(const StateOptions& opts, Args&&... pairs_of_key_ptr) : opts_(opts), state_()
  {
    preallocate();
 
    insertSystemStateElement(std::make_pair(
        SystemStateElementName::S,
        createSystemStateElement<CameraExtrinsicState>(std::make_tuple(opts.initial_camera_extrinsics_))));
 
    if (opts.enable_camera_intrinsics_calibration_)
    {
      insertSystemStateElement(std::make_pair(
          SystemStateElementName::K,
          createSystemStateElement<CameraIntrinsicState>(std::make_tuple(opts.initial_camera_intrinsics_))));
    }
 
    (insertSystemStateElement(std::forward<decltype(pairs_of_key_ptr)>(pairs_of_key_ptr)), ...);
  }

  SystemState(const SystemState& other);
  SystemState(SystemState&& other) noexcept;
  SystemState& operator=(const SystemState& other);
  SystemState& operator=(SystemState&& other) noexcept;
  ~SystemState();

  [[nodiscard]] const SE23& T() const;

  [[nodiscard]] const SE3 P() const;

  [[nodiscard]] const SE3 V() const;

  [[nodiscard]] const Vector6& b() const;

  [[nodiscard]] const SE3& S() const;

  [[nodiscard]] const In& K() const;

  [[nodiscard]] const Vector4 k() const;

  [[nodiscard]] const Vector3& f(const uint& feat_id) const;

  [[nodiscard]] const Vector3 ge3() const;

  [[nodiscard]] inline const StateOptions& opts() const { return opts_; }

  static std::string toString(const SystemStateKey& key);
 
 private:
  void preallocate();

  void insertSystemStateElement(std::pair<SystemStateKey, SystemStateElementUniquePtr>&& key_ptr);

  void insertSystemStateElement(std::vector<std::pair<SystemStateKey, SystemStateElementUniquePtr>>& keys_ptrs);
 
  friend class Symmetry;  
 
  StateOptions opts_;  
 
  SystemStateMap state_;  
};
 
}  // namespace msceqf
 
#endif  // SYSTEM_HPP
 
// [COMMENT] I can use unorderd map as soon as i check for out of order images when getting a image...