static_initializer.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 BSD-2-Clause-License with
// no commercial use allowed, the full terms of which are made available
// in the LICENSE file. No license in patents is granted.
//
// You can contact the authors at <alessandro.fornasier@ieee.org>
 
#ifndef STATIC_INITIALIZER_HPP
#define STATIC_INITIALIZER_HPP
 
#include "msceqf/filter/checker/checker.hpp"
#include "sensors/sensor_data.hpp"
 
namespace msceqf
{
class StaticInitializer
{
 public:
  using ImuBuffer = std::deque<Imu>;  
 
  StaticInitializer(const InitializerOptions& opts, const Checker& checker);
 
  void insertImu(const Imu& imu);
 
  [[nodiscard]] bool detectMotion(const Tracks& tracks);
 
  [[nodiscard]] bool initializeOrigin();
 
  [[nodiscard]] const SE23& T0() const;
 
  [[nodiscard]] const Vector6& b0() const;
 
 private:
  [[nodiscard]] bool detectAccelerationSpike();
 
  [[nodiscard]] bool imuMeanStd(Vector3& acc_mean, Vector3& ang_mean, fp& acc_std) const;
 
  void computeOrigin(Vector3& acc_mean, Vector3& ang_mean);
 
  InitializerOptions opts_;  
 
  const Checker& checker_;  // The MSCEqF checker
 
  ImuBuffer imu_buffer_;  
 
  SE23 T0_;     
  Vector6 b0_;  
};
 
}  // namespace msceqf
 
#endif  // STATIC_INITIALIZER_HPP