gyroscope.h

#ifndef CALICO_SENSORS_GYROSCOPE_H_
#define CALICO_SENSORS_GYROSCOPE_H_
 
#include <string>
#include <vector>
 
#include "Eigen/Dense"
#include "absl/container/flat_hash_map.h"
#include "absl/status/status.h"
#include "absl/strings/string_view.h"
#include "calico/sensors/gyroscope_models.h"
#include "calico/sensors/sensor_base.h"
#include "calico/trajectory.h"
#include "calico/typedefs.h"
#include "ceres/problem.h"
 
namespace calico::sensors {
 
struct GyroscopeObservationId {
  double stamp;
  int sequence;
 
  template <typename H>
  friend H AbslHashValue(H h, const GyroscopeObservationId& id) {
    return H::combine(std::move(h), id.stamp, id.sequence);
  }
  friend bool operator==(const GyroscopeObservationId& lhs,
                         const GyroscopeObservationId& rhs) {
    return (lhs.stamp == rhs.stamp && lhs.sequence == rhs.sequence);
  }
};
 
struct GyroscopeMeasurement {
  Eigen::Vector3d measurement;
  GyroscopeObservationId id;
};
 
class Gyroscope : public Sensor {
 public:
  explicit Gyroscope() = default;
  Gyroscope(const Gyroscope&) = delete;
  Gyroscope& operator=(const Gyroscope&) = delete;
  ~Gyroscope() = default;
 
  void SetName(const std::string& name);
  const std::string& GetName() const;
  void SetExtrinsics(const Pose3d& T_sensorrig_sensor);
  const Pose3d& GetExtrinsics() const;
  absl::Status SetIntrinsics(const Eigen::VectorXd& intrinsics);
  const Eigen::VectorXd& GetIntrinsics() const;
  absl::Status SetLatency(double latency);
  double GetLatency() const;
  void EnableExtrinsicsEstimation(bool enable);
  void EnableIntrinsicsEstimation(bool enable);
  void EnableLatencyEstimation(bool enable);
  void SetLossFunction(utils::LossFunctionType loss, double scale);
  absl::StatusOr<int> AddParametersToProblem(ceres::Problem& problem) final;
  absl::StatusOr<int> AddResidualsToProblem(ceres::Problem& problem,
                                            Trajectory& sensorrig_trajectory,
                                            WorldModel& world_model) final;
  absl::Status SetMeasurementNoise(double sigma) final;
  absl::Status UpdateResiduals(ceres::Problem& problem) final;
  void ClearResidualInfo() final;
 
 
  absl::StatusOr<std::vector<GyroscopeMeasurement>> Project(
      const std::vector<double>& interp_times,
      const Trajectory& sensorrig_trajectory,
      const WorldModel& world_model) const;
 
  absl::Status SetModel(GyroscopeIntrinsicsModel gyroscope_model);
 
  GyroscopeIntrinsicsModel GetModel() const;
 
 
  absl::Status AddMeasurement(const GyroscopeMeasurement& measurement);
 
 
  absl::Status AddMeasurements(
      const std::vector<GyroscopeMeasurement>& measurements);
 
  void ClearMeasurements();
 
  int NumberOfMeasurements() const;
 
 private:
  std::string name_;
  bool intrinsics_enabled_;
  bool extrinsics_enabled_;
  bool latency_enabled_;
  std::unique_ptr<GyroscopeModel> gyroscope_model_;
  Pose3d T_sensorrig_sensor_;
  Eigen::VectorXd intrinsics_;
  double latency_ = 0.0;
  double sigma_ = 1.0;
  utils::LossFunctionType loss_function_;
  double loss_scale_ = 1.0;
  absl::flat_hash_map<GyroscopeObservationId, GyroscopeMeasurement>
      id_to_measurement_;
  absl::flat_hash_map<GyroscopeObservationId, Eigen::Vector3d> id_to_residual_;
  absl::flat_hash_map<GyroscopeObservationId, ceres::ResidualBlockId>
      id_to_residual_id_;
};
 
}  // namespace calico::sensors
 
#endif  // CALICO_SENSORS_GYROSCOPE_H_