updater.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_HPP
#define UPDATER_HPP
 
#include <unordered_set>
 
#include "types/fptypes.hpp"
#include "msceqf/options/msceqf_options.hpp"
#include "msceqf/filter/updater/updater_helper.hpp"
#include "msceqf/system/system.hpp"
#include "vision/track.hpp"
 
namespace msceqf
{
class Updater
{
 public:
  Updater(const UpdaterOptions& opts, const SystemState& xi0);

  void mscUpdate(MSCEqFState& X, const Tracks& tracks, std::unordered_set<uint>& ids);
 
 private:
  [[nodiscard]] bool linearTriangulation(const MSCEqFState& X, const Track& track, const SE3& A_E, Vector3& A_f) const;

  void nonlinearTriangulation(const MSCEqFState& X, const Track& track, const SE3& A_E, Vector3& A_f) const;

  void nonlinearTriangulationResidualJacobian(
      const MSCEqFState& X, const Track& track, const SE3& A_E, const Vector3& A_f, VectorX& res, MatrixX& J) const;

  void UpdateMSCEqF(MSCEqFState& X, const MatrixX& C, const VectorX& delta, const MatrixX& R) const;
 
 private:
  UpdaterOptions opts_;  
 
  const SystemState& xi0_;  
 
  ProjectionHelperUniquePtr ph_;  
 
  std::map<uint, fp> chi2_table_;  
 
  ColsMap cols_map_;  
 
  std::vector<uint> update_ids_;  
  size_t total_size_;             
};
 
}  // namespace msceqf
 
#endif  // UPDATER_HPP