world_model.h

#ifndef CALICO_WORLD_MODEL_H_
#define CALICO_WORLD_MODEL_H_
 
#include <memory>
 
#include "calico/typedefs.h"
#include "absl/container/flat_hash_map.h"
#include "absl/status/status.h"
#include "ceres/problem.h"
#include "Eigen/Dense"
 
 
namespace calico {
 
constexpr int kLandmarkFrameId = -1;
 
struct Landmark {
  Eigen::Vector3d point;
  int id;
  bool point_is_constant;
};
 
struct RigidBody {
  std::unordered_map<int, Eigen::Vector3d> model_definition;
  Pose3d T_world_rigidbody;
  int id;
  bool world_pose_is_constant;
  bool model_definition_is_constant;
};
 
class WorldModel {
 public:
  static constexpr double kGravityDefaultZ = -9.80665;
 
  WorldModel();
  ~WorldModel();
 
  absl::Status AddLandmark(Landmark* landmark, bool take_ownership = true);
 
  absl::Status AddRigidBody(RigidBody* rigidbody, bool take_ownership = true);
 
  int AddParametersToProblem(ceres::Problem& problem);
 
  const absl::flat_hash_map<int, std::unique_ptr<Landmark>>& landmarks() const;
  absl::flat_hash_map<int, std::unique_ptr<Landmark>>& landmarks();
 
  const absl::flat_hash_map<int, std::unique_ptr<RigidBody>>& rigidbodies() const;
  absl::flat_hash_map<int, std::unique_ptr<RigidBody>>& rigidbodies();
 
  void EnableGravityEstimation(bool enable);
 
  const Eigen::Vector3d& gravity() const;
  Eigen::Vector3d& gravity();
 
  void SetGravity(const Eigen::Vector3d& gravity);
 
  const Eigen::Vector3d& GetGravity() const;
 
 
  int NumberOfLandmarks() const;
 
  int NumberOfRigidBodies() const;
 
  void ClearLandmarks();
 
  void ClearRigidBodies();
 
  void Clear();
 
 private:
  absl::flat_hash_map<int, bool> rigidbody_id_to_own_rigidbody_;
  absl::flat_hash_map<int, bool> landmark_id_to_own_landmark_;
  absl::flat_hash_map<int, std::unique_ptr<RigidBody>> rigidbody_id_to_rigidbody_;
  absl::flat_hash_map<int, std::unique_ptr<Landmark>> landmark_id_to_landmark_;
 
  Eigen::Vector3d gravity_;
  bool gravity_enabled_;
};
 
} // namespace calico
 
#endif // CALICO_WORLD_MODEL_H_