camera.h

#ifndef CALICO_SENSORS_CAMERA_H_
#define CALICO_SENSORS_CAMERA_H_
 
#include <string>
#include <vector>
 
#include "Eigen/Dense"
#include "absl/container/flat_hash_map.h"
#include "absl/container/flat_hash_set.h"
#include "absl/status/status.h"
#include "absl/strings/string_view.h"
#include "calico/sensors/camera_models.h"
#include "calico/sensors/sensor_base.h"
#include "calico/trajectory.h"
#include "calico/typedefs.h"
#include "ceres/problem.h"
 
namespace calico::sensors {
 
struct CameraObservationId {
  double stamp;
  int image_id;
  int model_id;
  int feature_id;
 
  template <typename H>
  friend H AbslHashValue(H h, const CameraObservationId& id) {
    return H::combine(std::move(h), id.stamp, id.image_id, id.model_id,
                      id.feature_id);
  }
  friend bool operator==(const CameraObservationId& lhs,
                         const CameraObservationId& rhs) {
    return (lhs.stamp == rhs.stamp && lhs.image_id == rhs.image_id &&
            lhs.model_id == rhs.model_id && lhs.feature_id == rhs.feature_id);
  }
};
 
// Camera measurement type.
struct CameraMeasurement {
  Eigen::Vector2d pixel;
  CameraObservationId id;
};
 
class Camera : public Sensor {
 public:
  explicit Camera() = default;
  Camera(const Camera&) = delete;
  Camera& operator=(const Camera&) = delete;
  ~Camera() = 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) final;
  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<CameraMeasurement>> Project(
      const std::vector<double>& interp_times,
      const Trajectory& sensorrig_trajectory,
      const WorldModel& world_model) const;
 
  absl::Status SetModel(CameraIntrinsicsModel camera_model);
 
  CameraIntrinsicsModel GetModel() const;
 
 
  absl::Status AddMeasurement(const CameraMeasurement& measurement);
 
 
  absl::Status AddMeasurements(
      const std::vector<CameraMeasurement>& measurements);
 
  const absl::flat_hash_map<CameraObservationId, CameraMeasurement>&
  GetMeasurementIdToMeasurement() const;
 
 
  absl::StatusOr<std::vector<std::pair<CameraMeasurement, Eigen::Vector2d>>>
  GetMeasurementResidualPairs() const;
 
 
  absl::Status MarkOutlierById(const CameraObservationId& id);
 
 
  absl::Status MarkOutliersById(const std::vector<CameraObservationId>& ids);
 
  void ClearOutliersList();
 
 
  void ClearMeasurements();
 
  int NumberOfMeasurements() const;
 
 private:
  std::string name_;
  bool intrinsics_enabled_;
  bool extrinsics_enabled_;
  bool latency_enabled_;
  std::unique_ptr<CameraModel> camera_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<CameraObservationId, CameraMeasurement>
      id_to_measurement_;
  absl::flat_hash_map<CameraObservationId, Eigen::Vector2d> id_to_residual_;
  absl::flat_hash_map<CameraObservationId, ceres::ResidualBlockId>
      id_to_residual_id_;
  absl::flat_hash_set<CameraObservationId> outlier_ids_;
};
 
}  // namespace calico::sensors
 
#endif  // CALICO_SENSORS_CAMERA_H_