state_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 STATE_ELEMENTS_HPP
#define STATE_ELEMENTS_HPP
 
#include <memory>
 
#include "types/fptypes.hpp"
 
namespace msceqf
{
enum class MSCEqFStateElementName
{
  Dd,  
  E,   
  L,   
};
 
class MSCEqFStateElement
{
 public:
  virtual ~MSCEqFStateElement() = default;
 
  [[nodiscard]] const uint& getIndex() { return idx_; }
 
  [[nodiscard]] const uint& getDof() { return dof_; }
 
  void updateIndex(const uint& new_idx) { idx_ = new_idx; }
 
  virtual void updateRight(const VectorX& delta) = 0;
 
  virtual void updateLeft(const VectorX& delta) = 0;
 
  virtual std::unique_ptr<MSCEqFStateElement> clone() const = 0;
 
 protected:
  MSCEqFStateElement() = delete;
  MSCEqFStateElement(const MSCEqFStateElement&) = default;
  MSCEqFStateElement(MSCEqFStateElement&&) = default;
  MSCEqFStateElement& operator=(const MSCEqFStateElement&) = default;
  MSCEqFStateElement& operator=(MSCEqFStateElement&&) = default;
 
  MSCEqFStateElement(const uint& idx, const uint& dof) : idx_(idx), dof_(dof){};
 
  uint idx_;  
  uint dof_;  
};
 
struct MSCEqFSDBState final : public MSCEqFStateElement
{
  MSCEqFSDBState() = delete;
 
  MSCEqFSDBState(const uint& idx) : MSCEqFStateElement(idx, 15), Dd_(){};
 
  void updateRight(const VectorX& delta) override { Dd_.multiplyRight(SDB::exp(delta)); }
 
  void updateLeft(const VectorX& delta) override { Dd_.multiplyLeft(SDB::exp(delta)); }
 
  std::unique_ptr<MSCEqFStateElement> clone() const override { return std::make_unique<MSCEqFSDBState>(*this); }
 
  SDB Dd_;  
};
 
struct MSCEqFSE3State final : public MSCEqFStateElement
{
  MSCEqFSE3State() = delete;
 
  MSCEqFSE3State(const uint& idx) : MSCEqFStateElement(idx, 6), E_(){};
 
  void updateRight(const VectorX& delta) override { E_.multiplyRight(SE3::exp(delta)); }
 
  void updateLeft(const VectorX& delta) override { E_.multiplyLeft(SE3::exp(delta)); }
 
  std::unique_ptr<MSCEqFStateElement> clone() const override { return std::make_unique<MSCEqFSE3State>(*this); }
 
  SE3 E_;  
};
 
struct MSCEqFInState final : public MSCEqFStateElement
{
  MSCEqFInState() = delete;
 
  MSCEqFInState(const uint& idx) : MSCEqFStateElement(idx, 4), L_(){};
 
  void updateRight(const VectorX& delta) override { L_.multiplyRight(In::exp(delta)); }
 
  void updateLeft(const VectorX& delta) override { L_.multiplyLeft(In::exp(delta)); }
 
  std::unique_ptr<MSCEqFStateElement> clone() const override { return std::make_unique<MSCEqFInState>(*this); }
 
  In L_;  
};
 
struct MSCEqFSOT3State final : public MSCEqFStateElement
{
  MSCEqFSOT3State() = delete;
 
  MSCEqFSOT3State(const uint& idx) : MSCEqFStateElement(idx, 4), Q_(){};
 
  void updateRight(const VectorX& delta) override { Q_.multiplyRight(SOT3::exp(delta)); }
 
  void updateLeft(const VectorX& delta) override { Q_.multiplyLeft(SOT3::exp(delta)); }
 
  std::unique_ptr<MSCEqFStateElement> clone() const override { return std::make_unique<MSCEqFSOT3State>(*this); }
 
  SOT3 Q_;  
};
 
using MSCEqFStateElementSharedPtr = std::shared_ptr<MSCEqFStateElement>;
using MSCEqFStateElementUniquePtr = std::unique_ptr<MSCEqFStateElement>;
using MSCEqFSDBStateSharedPtr = std::shared_ptr<MSCEqFSDBState>;
using MSCEqFSDBStateUniquePtr = std::unique_ptr<MSCEqFSDBState>;
using MSCEqFSE3StateSharedPtr = std::shared_ptr<MSCEqFSE3State>;
using MSCEqFSE3StateUniquePtr = std::unique_ptr<MSCEqFSE3State>;
using MSCEqFInStateSharedPtr = std::shared_ptr<MSCEqFInState>;
using MSCEqFInStateUniquePtr = std::unique_ptr<MSCEqFInState>;
using MSCEqFSOT3StateSharedPtr = std::shared_ptr<MSCEqFSOT3State>;
using MSCEqFSOT3StateUniquePtr = std::unique_ptr<MSCEqFSOT3State>;
 
template <typename T>
[[nodiscard]] static MSCEqFStateElementUniquePtr createMSCEqFStateElement(const uint& idx)
{
  if constexpr (std::is_base_of_v<MSCEqFStateElement, T>)
  {
    return std::make_unique<T>(idx);
  }
  else
  {
    return nullptr;
  }
}
 
}  // namespace msceqf
 
#endif  // STATE_ELEMENTS_HPP