msceqf.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 MSCEQF_HPP
#define MSCEQF_HPP
 
#include <future>
 
#include "msceqf/filter/initializer/static_initializer.hpp"
#include "msceqf/filter/propagator/propagator.hpp"
#include "msceqf/filter/updater/updater.hpp"
#include "msceqf/filter/updater/zero_velocity_updater.hpp"
#include "msceqf/options/msceqf_option_parser.hpp"
#include "msceqf/state/state.hpp"
#include "vision/track_manager.hpp"
#include "utils/visualizer.hpp"
 
namespace msceqf
{
class MSCEqF
{
 public:
  MSCEqF(const std::string& params_filepath);

  void processMeasurement(const Imu& meas) { processImuMeasurement(meas); }

  void processMeasurement(Camera& meas) { processCameraMeasurement(meas); }

  void processMeasurement(TriangulatedFeatures& meas) { processFeaturesMeasurement(meas); }

  const MSCEqFOptions& options() const;

  const StateOptions& stateOptions() const;

  const MatrixX& covariance() const;

  const MatrixX coreCovariance() const;

  const SystemState& stateEstimate() const;

  const SystemState& stateOrigin() const;

  void setGivenOrigin(const SE23& T0, const Vector6& b0, const fp& timestamp);

  const cv::Mat3b imageWithTracks(const Camera& cam) const;

  void visualizeImageWithTracks(const Camera& cam) const;

  [[nodiscard]] const bool& isInit() const;

  [[nodiscard]] const bool& zvuPerformed() const;
 
 private:
  void processImuMeasurement(const Imu& imu);

  void processCameraMeasurement(Camera& cam);

  void processFeaturesMeasurement(TriangulatedFeatures& features);

  void initialize(Camera& cam);

  void initialize(TriangulatedFeatures& features);

  void setGivenOrigin();

  void logInit() const;
 
  OptionParser parser_;  
  MSCEqFOptions opts_;   
 
  SystemState xi0_;  
  MSCEqFState X_;    
  SystemState xi_;   
 
  TrackManager track_manager_;     
  Checker checker_;                
  StaticInitializer initializer_;  
  Propagator propagator_;          
  Updater updater_;                
  ZeroVelocityUpdater zvupdater_;  
  Visualizer visualizer_;          //<! The MSCEqF visualizer
 
  std::unordered_set<uint> ids_to_update_;  
 
  fp timestamp_;  
 
  bool is_filter_initialized_;  
  bool zvu_performed_;          
};
 
}  // namespace msceqf
 
#endif  // MSCEQF_HPP