updater_helper.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 UPDATER_HELPER_HPP
#define UPDATER_HELPER_HPP
 
#include <opencv2/opencv.hpp>
#include <functional>
#include <boost/math/distributions/chi_squared.hpp>
 
#include "types/fptypes.hpp"
#include "msceqf/state/state.hpp"
#include "utils/tools.hpp"
 
namespace msceqf
{
using MatrixXBlockRowRef = Ref<MatrixX::RowsBlockXpr>;  
using VectorXBlockRowRef = Ref<VectorX::RowsBlockXpr>;  
 
using ColsMap = utils::InsertionOrderedMap<MSCEqFState::MSCEqFKey, size_t>;  

struct FeatHelper
{
  FeatHelper(
      const Vector3& A_f, const Vector2& uv, const Vector2& uvn, const fp& anchor_timestamp, const fp& clone_timestamp)
      : A_f_(A_f), uv_(uv), uvn_(uvn), anchor_timestamp_(anchor_timestamp), clone_timestamp_(clone_timestamp){};
 
  const Vector3& A_f_;          
  const Vector2& uv_;           
  const Vector2& uvn_;          
  const fp& anchor_timestamp_;  
  const fp& clone_timestamp_;   
};

class ProjectionHelper
{
 public:
  virtual ~ProjectionHelper() = default;

  [[nodiscard]] virtual Vector3 pi(const Vector3& f) = 0;

  [[nodiscard]] virtual MatrixX dpi(const Vector3& f) = 0;

  virtual void residualJacobianBlock(const MSCEqFState& X,
                                     const SystemState& xi0,
                                     const FeatHelper& feat,
                                     MatrixXBlockRowRef C_block_row,
                                     VectorXBlockRowRef delta_block_row,
                                     MatrixXBlockRowRef Cf_block_row,
                                     const ColsMap& cols_map) = 0;

  [[nodiscard]] const size_t& block_rows() const { return block_rows_; }

  [[nodiscard]] const size_t& dim_loss() const { return dim_loss_; }
 
 protected:
  ProjectionHelper(const FeatureRepresentation& feature_representation);
  ProjectionHelper(const ProjectionHelper&) = default;
  ProjectionHelper(ProjectionHelper&&) = default;
  ProjectionHelper& operator=(const ProjectionHelper&) = default;
  ProjectionHelper& operator=(ProjectionHelper&&) = default;
 
  FeatureRepresentation feature_representation_;  
 
  size_t block_rows_;  
  size_t dim_loss_;    
};

class ProjectionHelperS2 : public ProjectionHelper
{
 public:
  ProjectionHelperS2(const FeatureRepresentation& feature_representation) : ProjectionHelper(feature_representation)
  {
    block_rows_ = 3;
  }

  [[nodiscard]] Vector3 pi(const Vector3& f) override;

  [[nodiscard]] MatrixX dpi(const Vector3& f) override;

  void residualJacobianBlock(const MSCEqFState& X,
                             const SystemState& xi0,
                             const FeatHelper& feat,
                             MatrixXBlockRowRef C_block_row,
                             VectorXBlockRowRef delta_block_row,
                             MatrixXBlockRowRef Cf_block_row,
                             const ColsMap& cols_map) override;
};

class ProjectionHelperZ1 : public ProjectionHelper
{
 public:
  ProjectionHelperZ1(const FeatureRepresentation& feature_representation) : ProjectionHelper(feature_representation)
  {
    block_rows_ = 2;
  }

  [[nodiscard]] Vector3 pi(const Vector3& f) override;

  [[nodiscard]] MatrixX dpi(const Vector3& f) override;

  void residualJacobianBlock(const MSCEqFState& X,
                             const SystemState& xi0,
                             const FeatHelper& feat,
                             MatrixXBlockRowRef C_block_row,
                             VectorXBlockRowRef delta_block_row,
                             MatrixXBlockRowRef Cf_block_row,
                             const ColsMap& cols_map) override;
};
 
using ProjectionHelperSharedPtr = std::shared_ptr<ProjectionHelper>;
using ProjectionHelperUniquePtr = std::unique_ptr<ProjectionHelper>;
using ProjectionHelperZ1SharedPtr = std::shared_ptr<ProjectionHelperZ1>;
using ProjectionHelperZ1UniquePtr = std::unique_ptr<ProjectionHelperZ1>;
using ProjectionHelperS2SharedPtr = std::shared_ptr<ProjectionHelperS2>;
using ProjectionHelperS2UniquePtr = std::unique_ptr<ProjectionHelperS2>;

template <typename T>
[[nodiscard]] static ProjectionHelperUniquePtr createProjectionHelper(
    const FeatureRepresentation& feature_representation)
{
  if constexpr (std::is_base_of_v<ProjectionHelper, T>)
  {
    return std::make_unique<T>(feature_representation);
  }
  else
  {
    return nullptr;
  }
}

struct UpdaterHelper
{
  [[nodiscard]] static Matrix<2, 4> Xi(const Vector3& f);

  [[nodiscard]] static Matrix3 inverseDepthJacobian(const Vector3& A_f);

  static void nullspaceProjection(Ref<MatrixX> Cf, MatrixXBlockRowRef Ct, VectorXBlockRowRef delta);

  static void updateQRCompression(MatrixX& C, VectorX& delta);

  [[nodiscard]] static bool chi2Test(const fp& chi2, const size_t& dof, const std::map<uint, fp>& chi2_table);
};
 
}  // namespace msceqf
 
#endif  // UPDATER_HELPER_HPP