Program Listing for File vi-map-geometry.h

Return to documentation for file (algorithms/vi-map-helpers/include/vi-map-helpers/vi-map-geometry.h)


#include <vector>

#include <Eigen/Dense>
#include <posegraph/unique-id.h>
#include <vi-map/unique-id.h>
#include <vi-map/vi-map.h>

namespace vi_map {
class Vertex;
}  // namespace vi_map

namespace vi_map_helpers {

class VIMapGeometry {
  explicit VIMapGeometry(const vi_map::VIMap& map);

  // Positions, transformations etc.
  pose::Transformation getVisualFrame_T_G_C(
      const vi_map::VisualFrameIdentifier& frame_id) const;

  // Uses all good landmarks to compute median scene depth. If no landmarks are
  // found it returns infinity.
  double getMedianSceneDepth(
      const vi_map::VisualFrameIdentifier& frame_id) const;

  int getNeighboursWithinRange(
      const pose_graph::VertexId& vertex_id, double range_m,
      pose_graph::VertexIdSet* neighbours) const;

  void get_p_G_I_CovarianceEigenValuesAndVectorsAscending(
      const vi_map::MissionId& mission_id, Eigen::Vector3d* eigenvalues,
      Eigen::Matrix3d* eigenvectors) const;

  // Bearing vector from root vertex to average vertex position.
  Eigen::Vector3d get_bv_G_root_average(
      const vi_map::MissionId& mission_id) const;

  // Diverse bounding boxes.
  template <typename ObjectIdType>
  void getBoundingBox(Eigen::Vector3d* p_G_min, Eigen::Vector3d* p_G_max) const;
  template <typename ObjectIdType>
  void getBoundingBox(
      const vi_map::MissionId& mission_id, Eigen::Vector3d* p_G_min,
      Eigen::Vector3d* p_G_max) const;
  template <typename ObjectIdContainerType>
  void getBoundingBox(
      const ObjectIdContainerType& object_ids, Eigen::Vector3d* p_G_min,
      Eigen::Vector3d* p_G_max) const;

  // Use the IMU to interpolate T_G_I at any timestamp within the range of this
  // mission.
  void interpolateForTimestamps_T_G_I(
      const vi_map::MissionId& mission_id,
      const Eigen::Matrix<int64_t, 1, Eigen::Dynamic>& timestamps_ns,
      aslam::TransformationVector* T_G_I_vector) const;

  const vi_map::VIMap& map_;

}  // namespace vi_map_helpers

#include "vi-map-helpers/vi-map-geometry-inl.h"