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