msckf_updater.hpp

// Copyright 2023 Jacob Hartzer
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <https://www.gnu.org/licenses/>.
 
#ifndef EKF__UPDATE__MSCKF_UPDATER_HPP_
#define EKF__UPDATE__MSCKF_UPDATER_HPP_
 
#include <eigen3/Eigen/Eigen>
 
#include <memory>
#include <string>
#include <vector>
 
#include "ekf/types.hpp"
#include "ekf/update/updater.hpp"
#include "infrastructure/data_logger.hpp"
 
class MsckfUpdater : public Updater
{
public:
  explicit MsckfUpdater(
    unsigned int cam_id,
    bool is_extrinsic,
    const std::string & log_file_directory,
    double data_log_rate,
    double min_feat_dist,
    std::shared_ptr<DebugLogger> logger
  );
 
  bool TriangulateFeature(
    const double local_time,
    EKF & ekf,
    const FeatureTrack & feature_track,
    Eigen::Vector3d & pos_f_in_l);
 
  void UpdateEKF(
    EKF & ekf,
    const double time,
    const FeatureTracks & feature_tracks,
    double px_error);
 
  static void DistortionJacobian(
    const Eigen::Vector2d & xy_norm,
    const Intrinsics & intrinsics,
    Eigen::MatrixXd & H_d);
 
  static void ProjectionJacobian(const Eigen::Vector3d & position, Eigen::MatrixXd & jacobian);
 
  static Eigen::Vector2d Project(const Eigen::Vector3d pos_f_in_c);
 
  static Eigen::Vector2d Distort(const Eigen::Vector2d & xy_norm, const Intrinsics & intrinsics);
 
private:
  bool m_is_cam_extrinsic;
  DataLogger m_msckf_logger;
  DataLogger m_triangulation_logger;
  double m_min_feat_dist{1.0};
  double m_max_feat_dist{100.0};  
  bool m_is_first_estimate{true};
  bool m_use_true_triangulation{true};
  Intrinsics m_intrinsics;
  Eigen::Vector3d m_pos_c_in_b{0.0, 0.0, 0.0};
  Eigen::Quaterniond m_ang_c_to_b{1.0, 0.0, 0.0, 0.0};
};
 
#endif  // EKF__UPDATE__MSCKF_UPDATER_HPP_