state.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 STATE_HPP
#define STATE_HPP
 
#include <map>
#include <variant>
 
#include "msceqf/system/system.hpp"
#include "msceqf/options/msceqf_options.hpp"
#include "msceqf/state/state_elements.hpp"
 
namespace msceqf
{
class MSCEqFState
{
 public:
  using MSCEqFStateKey = std::variant<MSCEqFStateElementName, uint>;  
  using MSCEqFKey = std::variant<MSCEqFStateKey, fp>;                 
 
  using MSCEqFStateMap = std::unordered_map<MSCEqFStateKey, MSCEqFStateElementSharedPtr>;  
  using MSCEqFClonesMap = std::map<fp, MSCEqFStateElementSharedPtr>;                       

  MSCEqFState() = delete;

  MSCEqFState(const StateOptions& opts, const SystemState& xi0);

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

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

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

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

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

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

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

  [[nodiscard]] const SOT3& Q(const uint& feat_id) const;

  [[nodiscard]] const SE3& clone(const fp& timestamp) const;

  [[nodiscard]] const uint& index(const MSCEqFKey& key) const;

  [[nodiscard]] const uint& dof(const MSCEqFKey& key) const;

  [[nodiscard]] inline size_t clonesSize() const { return clones_.size(); }

  [[nodiscard]] const fp& cloneTimestampToMarginalize() const;

  [[nodiscard]] const MatrixX& cov() const;

  [[nodiscard]] const MatrixX covBlock(const MSCEqFKey& key) const;

  [[nodiscard]] const MatrixX subCov(const std::vector<MSCEqFKey>& keys) const;

  [[nodiscard]] const MatrixX subCovCols(const std::vector<MSCEqFKey>& keys) const;

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

  void initializeStateElement(const MSCEqFStateKey& key, const MatrixX& cov_block);

  void stochasticCloning(const fp& timestamp);

  void marginalizeCloneAt(const fp& timestamp);

  static std::string toString(const MSCEqFStateKey& key);

  [[nodiscard]] const MSCEqFState Random() const;

  [[nodiscard]] const MSCEqFState operator*(const MSCEqFState& other) const;
 
 private:
  void preallocate();

  [[nodiscard]] bool insertStateElement(const MSCEqFStateKey& key, MSCEqFStateElementUniquePtr ptr);

  [[nodiscard]] bool insertCloneElement(const fp& timestamp, MSCEqFStateElementUniquePtr ptr);

  [[nodiscard]] const MSCEqFStateElementSharedPtr& getPtr(const MSCEqFKey& key) const;
 
  friend class Symmetry;             
  friend class Propagator;           
  friend class Updater;              
  friend class ZeroVelocityUpdater;  
 
  StateOptions opts_;  
 
  MatrixX cov_;             
  MSCEqFStateMap state_;    
  MSCEqFClonesMap clones_;  
};
 
}  // namespace msceqf
 
#endif  // STATE_HPP