accelerometer.h

#ifndef CALICO_SENSORS_ACCELEROMETER_H_
#define CALICO_SENSORS_ACCELEROMETER_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/accelerometer_models.h"
#include "calico/sensors/sensor_base.h"
#include "calico/trajectory.h"
#include "calico/typedefs.h"
#include "ceres/problem.h"
 
namespace calico::sensors {
 
struct AccelerometerObservationId {
  double stamp;
  int sequence;
 
  template <typename H>
  friend H AbslHashValue(H h, const AccelerometerObservationId& id) {
    return H::combine(std::move(h), id.stamp, id.sequence);
  }
  friend bool operator==(const AccelerometerObservationId& lhs,
                         const AccelerometerObservationId& rhs) {
    return (lhs.stamp == rhs.stamp && lhs.sequence == rhs.sequence);
  }
};
 
struct AccelerometerMeasurement {
  Eigen::Vector3d measurement;
  AccelerometerObservationId id;
};
 
class Accelerometer : public Sensor {
 public:
  explicit Accelerometer() = default;
  Accelerometer(const Accelerometer&) = delete;
  Accelerometer& operator=(const Accelerometer&) = delete;
  ~Accelerometer() = 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<AccelerometerMeasurement>> Project(
      const std::vector<double>& interp_times,
      const Trajectory& sensorrig_trajectory,
      const WorldModel& world_model) const;
 
  absl::Status SetModel(AccelerometerIntrinsicsModel accelerometer_model);
 
  AccelerometerIntrinsicsModel GetModel() const;
 
 
  absl::Status AddMeasurement(const AccelerometerMeasurement& measurement);
 
 
  absl::Status AddMeasurements(
      const std::vector<AccelerometerMeasurement>& measurements);
 
  void ClearMeasurements();
 
  int NumberOfMeasurements() const;
 
 private:
  std::string name_;
  bool intrinsics_enabled_;
  bool extrinsics_enabled_;
  bool latency_enabled_;
  std::unique_ptr<AccelerometerModel> accelerometer_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<AccelerometerObservationId, AccelerometerMeasurement>
      id_to_measurement_;
  absl::flat_hash_map<AccelerometerObservationId, Eigen::Vector3d>
      id_to_residual_;
  absl::flat_hash_map<AccelerometerObservationId, ceres::ResidualBlockId>
      id_to_residual_id_;
};
 
}  // namespace calico::sensors
 
#endif  // CALICO_SENSORS_ACCELEROMETER_H_