msceqf_option_parser.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 OPTIONS_PARSER_HPP
#define OPTIONS_PARSER_HPP
 
#include <yaml-cpp/yaml.h>
 
#include <numeric>
#include <sstream>
#include <opencv2/opencv.hpp>
 
#include "msceqf/options/msceqf_options.hpp"
#include "types/fptypes.hpp"
#include "utils/logger.hpp"
#include "utils/tools.hpp"
 
namespace msceqf
{
  class OptionParser
  {
  public:
    OptionParser(const std::string &filepath);

    MSCEqFOptions parseOptions();
 
  private:
    template <int Rows, int Cols>
    [[nodiscard]] bool read(Matrix<Rows, Cols> &x, const std::string &param)
    {
      if (node_[param])
      {
        using vector = std::vector<fp>;
        using vectorvector = std::vector<std::vector<fp>>;
 
        int rows = Rows;
        int cols = Cols;
 
        if constexpr (Rows == 1 || Cols == 1)
        {
          vector vec = node_[param].as<vector>();
          if constexpr (Rows == -1)
          {
            rows = vec.size();
          }
          else if (Cols == -1)
          {
            cols = vec.size();
          }
          x = Map<Matrix<Rows, Cols>>(vec.data(), rows, cols);
        }
        else
        {
          vectorvector mat = node_[param].as<vectorvector>();
          vector vec = utils::flatten(mat);
          if constexpr (Rows == 1 && Cols == 1)
          {
            rows = mat.size();
            cols = vec.size() / mat.size();
          }
          x = Map<Matrix<Rows, Cols, Eigen::RowMajor>>(vec.data(), rows, cols);
        }
        if constexpr (Rows == 1)
        {
          utils::Logger::info("Parameter: [" + param + "] found. Option set to:" +
                              (std::ostringstream{} << x).str());
        }
        else if (Cols == 1)
        {
          utils::Logger::info("Parameter: [" + param + "] found. Option set to: " +
                              (std::ostringstream{} << x.transpose()).str());
        }
        else
        {
          utils::Logger::info("Parameter: [" + param + "] found. Option set to: \n" +
                              (std::ostringstream{} << x).str());
        }
 
        return true;
      }
      utils::Logger::warn("Parameter: [" + param + "] not found");
      return false;
    }

    template <typename Scalar>
    [[nodiscard]] bool read(Quaternion &q, const std::string &param)
    {
      if (node_[param])
      {
        using vector = std::vector<fp>;
        vector vec = node_[param].as<vector>();
        q = Quaternion(vec).normalize();
        utils::Logger::info("Parameter: [" + param + "] found. Option set to: \n" +
                            (std::ostringstream{} << q).str());
        return true;
      }
      utils::Logger::warn("Parameter: [" + param + "] not found");
      return false;
    }

    template <typename T>
    [[nodiscard]] bool read(T &p, const std::string &param)
    {
      if (node_[param])
      {
        p = node_[param].as<T>();
        utils::Logger::info("Parameter: [" + param + "] found. Option set to: " +
                            (std::ostringstream{} << p).str());
        return true;
      }
      utils::Logger::warn("Parameter: [" + param + "] not found");
      return false;
    }
 
    template <typename T, typename Convertible>
    void readDefault(T &p, const Convertible &def, const std::string &param)
    {
      static_assert(std::is_convertible_v<Convertible, T>);
      if (!read(p, param))
      {
        p = def;
        utils::Logger::warn("Parameter: [" + param + "] set to default value: " +
                            (std::ostringstream{} << p).str());
      }
    }

    void parseCameraParameters(SE3 &extrinsics,
                               In &intrinsics,
                               DistortionModel &distortion_model,
                               VectorX &distortion_coefficients,
                               Vector2 &resolution,
                               fp &timeshift_cam_imu,
                               cv::Mat &mask,
                               MaskType &mask_type);

    void parseGivenOrigin(SE23 &T, Vector6 &b, fp &t0);

    void parseEqualizationMethod(EqualizationMethod &eq);

    void parseFeatureRepresentation(FeatureRepresentation &rep);

    void parseProjectionMethod(ProjectionMethod &proj);

    void parseDetectorType(FeatureDetector &detector);

    void parseInitialCovariance(Matrix9 &D_cov, Matrix6 &delta_cov, Matrix6 &E_cov, Matrix4 &L_cov);

    void parseProcessNoise(fp &w_std, fp &a_std, fp &bw_std, fp &ba_std);

    void parsePixStd(fp &pix_std, const StateOptions &opts);

    void parseZeroVelocityUpdate(ZeroVelocityUpdate &zvu);
 
    YAML::Node node_;      
    std::string filepath_; 
  };
 
} // namespace msceqf
 
#endif // OPTIONS_HPP