Class VIMap¶
Defined in File vi-map.h
Inheritance Relationships¶
Base Types¶
public backend::ResourceMap
(Class ResourceMap)public backend::MapInterface< vi_map::VIMap >
(Template Struct MapInterface)
Class Documentation¶
-
class
vi_map
::
VIMap
: public backend::ResourceMap, public backend::MapInterface<vi_map::VIMap>¶ Public Types
-
typedef Eigen::Matrix<unsigned char, Eigen::Dynamic, 1>
DescriptorType
¶
-
typedef Eigen::Matrix<unsigned char, Eigen::Dynamic, Eigen::Dynamic>
DescriptorsType
¶
-
typedef std::unordered_map<vi_map::MissionId, unsigned int>
MissionToLandmarkCountMap
¶
Public Functions
-
explicit
VIMap
(const std::string &map_folder)¶
-
explicit
VIMap
(const metadata::proto::MetaData &metadata_proto)¶
-
VIMap
()¶
-
virtual
~VIMap
()¶
-
bool
hexStringToMissionIdIfValid
(const std::string &map_mission_id_string, vi_map::MissionId *mission_id) const¶
-
bool
ensureMissionIdValid
(const std::string &map_mission_id_string, vi_map::MissionId *mission_id) const¶
-
template<typename
IdType
>
voidgetAllIdsInMission
(const MissionId &mission, std::vector<IdType> *all_ids_of_type) const¶
-
template<typename
IdType
>
voidgetMissionIds
(const IdType &object_id, vi_map::MissionIdSet *mission_ids) const¶
-
template<typename
IdType
>
voidgetMissionIds
(const std::vector<IdType> &object_ids, vi_map::MissionIdSet *mission_ids) const¶
-
template<typename
IdType
>
voidgetMissionIds
(const std::unordered_set<IdType> &object_ids, vi_map::MissionIdSet *mission_ids) const¶
-
inline const SensorManager &
getSensorManager
() const¶
-
inline SensorManager &
getSensorManager
()¶
-
inline size_t
numVertices
() const¶
-
inline size_t
numVerticesInMission
(const vi_map::MissionId &mission_id) const¶
-
inline bool
hasVertex
(const pose_graph::VertexId &id) const¶
-
inline size_t
numEdges
() const¶
-
inline bool
hasEdge
(const pose_graph::EdgeId &id) const¶
-
void
addNewMissionWithBaseframe
(const vi_map::MissionId &mission_id, const pose::Transformation &T_G_M, const Eigen::Matrix<double, 6, 6> &T_G_M_covariance, Mission::BackBone backbone_type)¶ Creates and adds a new MissionBaseFrame and a new Mission.
It does not set the mission root vertex id, so make sure to set this using mission->setRootVertexId(id) once the first vertex is created.
-
void
addNewMissionWithBaseframe
(vi_map::VIMission::UniquePtr mission, const vi_map::MissionBaseFrame &mission_base_frame)¶
-
inline size_t
numMissions
() const¶
-
inline bool
hasMission
(const vi_map::MissionId &id) const¶
-
void
associateMissionSensors
(const aslam::SensorIdSet &sensor_ids, const vi_map::MissionId &id)¶
-
void
getAllAssociatedMissionSensorIds
(const vi_map::MissionId &id, aslam::SensorIdSet *sensor_ids) const¶
-
const vi_map::Odometry6DoF &
getMissionOdometry6DoFSensor
(const vi_map::MissionId &id) const¶
-
vi_map::Odometry6DoF::Ptr
getMissionOdometry6DoFSensorPtr
(const vi_map::MissionId &id) const¶
-
const vi_map::LoopClosureSensor &
getMissionLoopClosureSensor
(const vi_map::MissionId &id) const¶
-
vi_map::LoopClosureSensor::Ptr
getMissionLoopClosureSensorPtr
(const vi_map::MissionId &id) const¶
-
const vi_map::Absolute6DoF &
getMissionAbsolute6DoFSensor
(const vi_map::MissionId &id) const¶
-
vi_map::Absolute6DoF::Ptr
getMissionAbsolute6DoFSensorPtr
(const vi_map::MissionId &id) const¶
-
const vi_map::WheelOdometry &
getMissionWheelOdometrySensor
(const vi_map::MissionId &id) const¶
-
vi_map::WheelOdometry::Ptr
getMissionWheelOdometrySensorPtr
(const vi_map::MissionId &id) const¶
-
inline size_t
numMissionBaseFrames
() const¶
-
inline bool
hasMissionBaseFrame
(const vi_map::MissionBaseFrameId &id) const¶
-
inline vi_map::MissionBaseFrame &
getMissionBaseFrame
(const vi_map::MissionBaseFrameId &id)¶
-
inline const vi_map::MissionBaseFrame &
getMissionBaseFrame
(const vi_map::MissionBaseFrameId &id) const¶
-
inline vi_map::MissionBaseFrame &
getMissionBaseFrameForMission
(const vi_map::MissionId &id)¶
-
inline const vi_map::MissionBaseFrame &
getMissionBaseFrameForMission
(const vi_map::MissionId &id) const¶
-
size_t
getNumAbsolute6DoFMeasurementsInMission
(const vi_map::MissionId &mission_id) const¶
-
inline size_t
numLandmarksInIndex
() const¶
-
size_t
numLandmarks
() const¶
-
inline bool
hasLandmark
(const vi_map::LandmarkId &id) const¶
-
void
addLandmarkIndexReference
(const vi_map::LandmarkId &landmark_id, const pose_graph::VertexId &storing_vertex_id)¶
-
inline void
updateLandmarkIndexReference
(const vi_map::LandmarkId &landmark_id, const pose_graph::VertexId &storing_vertex_id)¶ Set the storing information of a landmark id.
-
inline const vi_map::Vertex &
getLandmarkStoreVertex
(const vi_map::LandmarkId &id) const¶ Retrieve the vertex that given landmark is stored in.
-
inline vi_map::Vertex &
getLandmarkStoreVertex
(const vi_map::LandmarkId &id)¶ Retrieve the vertex that given landmark is stored in.
-
pose_graph::VertexId
getLandmarkStoreVertexId
(const vi_map::LandmarkId &id) const¶ Retrieve the id of the vertex that given landmark is stored in.
-
inline void
getAllLandmarkIds
(LandmarkIdSet *landmark_ids) const¶
-
inline void
getAllLandmarkIds
(LandmarkIdList *landmark_ids) const¶
-
inline void
forEachLandmark
(const std::function<void(const Landmark&)> &action) const¶ Executes the provided function on every landmark in the map.
Landmarks should not be deleted using this method.
-
inline void
forEachLandmark
(const std::function<void(const LandmarkId&, const Landmark&, const Vertex&, const MissionBaseFrame&, size_t landmark_counter)> &action) const¶ Executes the provided function on every landmark in the map.
Landmarks should not be deleted using this method.
-
pose_graph::Edge::EdgeType
getGraphTraversalEdgeType
(const MissionId &mission_id) const¶ Returns the main graph edge type which connects all vertices.
This is usually IMU or odometry data. This is the edge type which is used to traverse the graph to reach all vertices.
-
inline void
getObserverMissionsForLandmark
(const vi_map::LandmarkId &landmark_id, vi_map::MissionIdSet *missions) const¶ Get all the mission ids that contain frames that observe a given landmark.
-
inline void
getObserverVerticesForLandmark
(const vi_map::LandmarkId &landmark_id, pose_graph::VertexIdSet *observer_vertices) const¶ Get all vertices which contain frames that observe a given landmark.
-
inline void
getVisualFrameIdentifiersForLandmark
(const vi_map::LandmarkId &landmark_id, vi_map::VisualFrameIdentifierSet *observer_frames) const¶ Get all frames that observe a given landmark.
-
inline unsigned int
numLandmarkObserverMissions
(const vi_map::LandmarkId &landmark_id) const¶ Get the number of missions that contain frames that observe a given landmark.
-
unsigned int
numExpectedLandmarkObserverMissions
(const vi_map::LandmarkId &landmark_id) const¶ Return the number of missions that we expect to see this landmark given that they also see co-observed landmarks from other frames.
-
void
addNewLandmark
(const vi_map::Landmark &landmark, const pose_graph::VertexId &keypoint_and_store_vertex_id, unsigned int frame_index, unsigned int keypoint_index)¶ Add a new landmark to the map by providing the vertex, frame and keypoint information of the first observation.
-
void
addNewLandmark
(const LandmarkId &predefined_landmark_id, const KeypointIdentifier &first_observation)¶
-
void
associateKeypointWithExistingLandmark
(const KeypointIdentifier &keypoint_id, const LandmarkId &landmark_id)¶ Add a new observation to an existing landmark by providing information about the vertex, frame and keypoint index of the observation.
-
void
associateKeypointWithExistingLandmark
(const pose_graph::VertexId &keypoint_vertex_id, unsigned int frame_index, unsigned int keypoint_index, const LandmarkId &landmark_id)¶
-
inline const vi_map::MissionId &
getMissionIdForVertex
(const pose_graph::VertexId &id) const¶
-
inline const vi_map::MissionId &
getMissionIdForLandmark
(const vi_map::LandmarkId &id) const¶ Get the mission id of the vertex that this landmark is stored in.
-
inline const vi_map::VIMission &
getMissionForLandmark
(const vi_map::LandmarkId &id) const¶ Get the mission of the vertex that this landmark is stored in.
-
inline vi_map::VIMission &
getMissionForLandmark
(const vi_map::LandmarkId &id)¶ Get the mission of the vertex that this landmark is stored in.
-
inline vi_map::MissionBaseFrame &
getMissionBaseFrameForVertex
(const pose_graph::VertexId &id)¶ Get the baseframe of the mission that a given vertex belongs to.
-
inline const vi_map::MissionBaseFrame &
getMissionBaseFrameForVertex
(const pose_graph::VertexId &id) const¶ Get the baseframe of the mission that a given vertex belongs to.
-
inline Eigen::Vector3d
getLandmark_LM_p_fi
(const vi_map::LandmarkId &id) const¶
-
inline void
setLandmark_LM_p_fi
(const vi_map::LandmarkId &landmark_id, const Eigen::Vector3d &LM_p_fi)¶
-
inline Eigen::Vector3d
getLandmark_G_p_fi
(const vi_map::LandmarkId &id) const¶
-
inline Eigen::Vector3d
getLandmark_p_I_fi
(const vi_map::LandmarkId &landmark_id, const vi_map::Vertex &observer_vertex) const¶
-
inline Eigen::Vector3d
getLandmark_p_C_fi
(const LandmarkId &landmark_id, const Vertex &observer_vertex, unsigned int frame_idx) const¶
-
inline Eigen::Vector3d
getVertex_G_p_I
(const pose_graph::VertexId &vertex_id) const¶
-
inline Eigen::Quaterniond
getVertex_G_q_I
(const pose_graph::VertexId &vertex_id) const¶
-
inline pose::Transformation
getVertex_T_G_I
(const pose_graph::VertexId &vertex_id) const¶
-
void
getAllVertex_p_G_I
(const MissionId &mission_id, Eigen::Matrix3Xd *result) const¶
-
void
getVertex_p_G_I_ForVertexSet
(const pose_graph::VertexIdSet &vertex_ids, Eigen::Matrix3Xd *p_G_Is) const¶
-
inline void
getLandmarkDescriptors
(const LandmarkId &id, DescriptorsType *result) const¶
-
inline void
getMissionLandmarkCounts
(MissionToLandmarkCountMap *mission_to_landmark_count) const¶ Return a mapping from mission id to number of landmarks stored in vertices from this mission.
-
inline bool
getNextVertex
(const pose_graph::VertexId ¤t_vertex_id, pose_graph::Edge::EdgeType edge_type, pose_graph::VertexId *next_vertex_id) const¶ Provide the next vertex id by following the graph-traversal edge type of the graph.
-
inline bool
getPreviousVertex
(const pose_graph::VertexId ¤t_vertex_id, pose_graph::Edge::EdgeType edge_type, pose_graph::VertexId *previous_vertex_id) const¶ Provide the previous vertex id by following the graph-traversal edge type of the graph.
-
inline void
getAllVertexIds
(pose_graph::VertexIdList *vertices) const¶
-
inline void
forEachVisualFrame
(const std::function<void(const aslam::VisualFrame&, Vertex&, size_t, const MissionBaseFrame&)> &action)¶
-
inline void
forEachVisualFrame
(const std::function<void(const aslam::VisualFrame&, const Vertex&, size_t)> &action) const¶
-
inline void
forEachVisualFrame
(const std::function<void(const VisualFrameIdentifier&)> &action) const¶
-
inline void
forEachListedVisualFrame
(const VisualFrameIdentifierList &list, const std::function<void(const aslam::VisualFrame&, const Vertex&, const size_t, const MissionBaseFrame&)> &action) const¶
-
unsigned int
getVertexCountInMission
(const vi_map::MissionId &mission_id) const¶
-
void
getAllVertexIdsInMissionAlongGraph
(const vi_map::MissionId &mission_id, pose_graph::VertexIdList *vertices) const¶ Get all the vertex ids in the order that they appear when traversing the pose-graph of the provided mission.
-
void
getAllVertexIdsInMissionAlongGraph
(const vi_map::MissionId &mission_id, const pose_graph::VertexId &starting_verte_id, pose_graph::VertexIdList *vertices) const¶
-
void
getAllVertexIdsAlongGraphsSortedByTimestamp
(pose_graph::VertexIdList *vertices) const¶ Get all the vertex ids in the order that they appear when traversing the pose-graphs of all missions (sorted by timestamp).
-
void
getVertexIdsByMission
(vi_map::MissionVertexIdList *mission_to_vertex_ids_map) const¶
-
void
getAllVertexIdsInMission
(const vi_map::MissionId &mission_id, pose_graph::VertexIdList *vertices) const¶
-
void
getAllEdgeIdsInMissionAlongGraph
(const vi_map::MissionId &mission_id, pose_graph::EdgeIdList *edges) const¶
-
void
getAllEdgeIdsInMissionAlongGraph
(const vi_map::MissionId &mission_id, pose_graph::Edge::EdgeType edge_type, pose_graph::EdgeIdList *edges) const¶
-
void
getAllLandmarkIdsInMission
(const MissionId &mission_id, LandmarkIdList *landmarks) const¶
-
inline void
getAllEdgeIds
(pose_graph::EdgeIdList *edges) const¶
-
inline void
getAllMissionIds
(vi_map::MissionIdList *mission_ids) const¶
-
inline void
getAllMissionIds
(vi_map::MissionIdSet *mission_ids) const¶
-
inline void
getAllMissionIdsSortedByTimestamp
(vi_map::MissionIdList *sorted_mission_ids) const¶
-
void
forEachMission
(const std::function<void(const MissionId&)> &action) const¶
-
inline void
getAllMissionBaseFrameIds
(vi_map::MissionBaseFrameIdList *frames) const¶
-
inline void
getAllLandmarkIdsObservedAtVertices
(const pose_graph::VertexIdSet &vertex_ids, vi_map::LandmarkIdSet *observed_landmarks) const¶
-
inline void
removeLandmark
(const LandmarkId landmark_id)¶
-
inline void
removeVertex
(pose_graph::VertexId vertex_id)¶
-
inline void
removeEdge
(pose_graph::EdgeId edge_id)¶
-
inline size_t
removeLoopClosureEdges
()¶
-
inline vi_map::MissionId
getIdOfFirstMission
() const¶
-
void
selectMissions
(const vi_map::MissionIdSet &selected_missions_ids) const¶ Mission selection allows to limit all map operations to consider only data from a subset of missions (the selected missions).
Until mission-selection is reset the map will appear as if it would only contain data from the selected missions. Select the missions provided in the set of missions to select.
-
void
deselectMission
(const vi_map::MissionId &mission_id) const¶ Remove a provided mission from the set of selected missions.
-
void
resetMissionSelection
() const¶ Reset the mission selection: All missions in the map are considered.
-
unsigned int
numSelectedMissions
() const¶
-
const vi_map::MissionIdSet &
getSelectedMissions
() const¶
-
void
mergeLandmarks
(const vi_map::LandmarkId landmark_id_to_merge, const vi_map::LandmarkId &landmark_id_into)¶ Merge the two provided landmarks by changing all the back-references pointing to the “to-merge” landmark to point now to the “into” landmark.
Then the “to-merge” landmark is deleted.
-
void
moveLandmarkToOtherVertex
(const LandmarkId &landmark_id, const pose_graph::VertexId &vertex_id_to)¶ Moves a given landmark to be stored in the “to” vertex and updating all the references to it.
-
void
moveLandmarksToOtherVertex
(const pose_graph::VertexId &vertex_id_from, const pose_graph::VertexId &vertex_id_to)¶ Move all landmarks stored in the “from” vertex to the “to” vertex and updating all the references to it.
-
void
sparsifyMission
(const vi_map::MissionId &id, int every_nth_vertex_to_keep)¶
-
void
getDistanceTravelledPerMission
(const vi_map::MissionId &id, double *distance) const¶
-
bool
getEarliestMissionStartTimeNs
(int64_t *start_time_ns) const¶
-
void
getStatisticsOfMission
(const vi_map::MissionId &mission_id, std::vector<size_t> *num_good_landmarks_per_camera, std::vector<size_t> *num_bad_landmarks_per_camera, std::vector<size_t> *num_unknown_landmarks_per_camera, std::vector<size_t> *total_num_landmarks_per_camera, size_t *num_landmarks, size_t *num_vertices, size_t *num_observations, double *duration_s, int64_t *start_time, int64_t *end_time) const¶
-
std::string
printMapStatistics
(const vi_map::MissionId &mission, const unsigned int mission_number) const¶
-
std::string
printMapStatistics
(void) const¶
-
std::string
printMapAccumulatedStatistics
() const¶
-
void
printNumSensorResourcesOfMission
(const VIMission &mission, const std::function<void(const std::string&, const std::string&, int)> &print_aligned_function) const¶
-
void
mergeNeighboringVertices
(const pose_graph::VertexId &vertex_id_from, const pose_graph::VertexId &vertex_id_to, bool merge_viwls_edges = true, bool merge_odometry_edges = true, bool merge_wheel_odometry_edges = true)¶ Merge two vertices which are directly connected in the pose-graph (neighbors) by moving the landmarks from the “from” vertex to the “to” vertex.
The “from” vertex is then deleted.
-
template<typename
Edge
, vi_map::Edge::EdgeTypeEdgeType
>
size_tmergeEdgesOfNeighboringVertices
(const pose_graph::VertexId &merge_into_vertex_id, const pose_graph::VertexId &vertex_to_merge)¶
-
const vi_map::MissionId
duplicateMission
(const vi_map::MissionId &source_mission_id)¶
-
void
removeMissionObject
(const vi_map::MissionId &mission_id, bool remove_baseframe)¶
-
void
removeMission
(const vi_map::MissionId &mission_id, bool remove_baseframe)¶
-
void
getOutgoingOfType
(const pose_graph::Edge::EdgeType edge_type, const pose_graph::VertexId ¤t_vertex_id, pose_graph::EdgeIdList *edge_ids) const¶ Get the ids of outgoing edges to the provided vertex if they are of the provided type.
-
void
getIncomingOfType
(const pose_graph::Edge::EdgeType ref_edge_type, const pose_graph::VertexId ¤t_vertex_id, pose_graph::EdgeIdList *edge_ids) const¶ Get the ids of incoming edges to the provided vertex if they are of the provided type.
-
void
getRandomVertexId
(pose_graph::VertexId *vertex_id) const¶ Get an id of a random vertex in the graph, random generator can be controlled with the seed below.
-
void
getRandomVertexIdInMission
(const vi_map::MissionId &mission_id, pose_graph::VertexId *vertex_id) const¶ Get an id of a random vertex in the mission, random generator can be controlled with the seed below.
-
pose_graph::VertexId
getLastVertexIdOfMission
(const vi_map::MissionId &mission_id) const¶
-
void
setRandIntGeneratorSeed
(int seed)¶ Set the seed for random selection methods in this class.
-
bool
checkResourceConsistency
() const¶
-
bool
hasMissionResource
(const backend::ResourceType &resource_type, const MissionIdList &involved_mission_ids) const¶
-
template<typename
DataType
>
boolgetMissionResource
(const backend::ResourceType &type, const MissionIdList &involved_mission_ids, DataType *resource) const¶
-
template<typename
DataType
>
voidstoreMissionResource
(const backend::ResourceType &type, const DataType &resource, const MissionIdList &involved_mission_ids)¶
-
template<typename
DataType
>
voiddeleteMissionResource
(const backend::ResourceType &resource_type, const MissionIdList &involved_mission_ids)¶
-
template<typename
DataType
>
voidreplaceMissionResource
(const backend::ResourceType &resource_type, const DataType &resource, const MissionIdList &involved_mission_ids)¶
-
template<typename
DataType
>
voidstoreFrameResourceToFolder
(const DataType &resource, const std::string &resource_folder, const unsigned int frame_idx, const backend::ResourceType &type, Vertex *vertex_ptr)¶
-
template<typename
DataType
>
voidstoreFrameResource
(const DataType &resource, const unsigned int frame_idx, const backend::ResourceType &type, Vertex *vertex_ptr)¶
-
template<typename
DataType
>
boolgetFrameResource
(const Vertex &vertex, const unsigned int frame_idx, const backend::ResourceType &type, DataType *resource) const¶
-
bool
hasFrameResource
(const Vertex &vertex, const unsigned int frame_idx, const backend::ResourceType &type) const¶
-
template<typename
DataType
>
voidreplaceFrameResource
(const DataType &resource, const unsigned int frame_idx, const backend::ResourceType &type, Vertex *vertex_ptr)¶
-
template<typename
DataType
>
voiddeleteFrameResourcesOfType
(const unsigned int frame_idx, const backend::ResourceType &type, Vertex *vertex_ptr)¶
-
void
deleteAllFrameResources
(const unsigned int frame_idx, const bool delete_from_file_system, Vertex *vertex_ptr)¶
-
bool
hasSensorResource
(const VIMission &mission, const backend::ResourceType &type, const aslam::SensorId &sensor_id, const int64_t timestamp_ns) const¶
-
bool
hasSensorResource
(const MissionIdList &involved_mission_ids, const backend::ResourceType &type) const¶
-
template<typename
DataType
>
boolgetSensorResource
(const VIMission &mission, const backend::ResourceType &type, const aslam::SensorId &sensor_id, const int64_t timestamp_ns, DataType *resource) const¶
-
template<typename
DataType
>
boolgetClosestSensorResource
(const VIMission &mission, const backend::ResourceType &type, const aslam::SensorId &sensor_id, const int64_t timestamp_ns, const int64_t tolerance_ns, DataType *resource, int64_t *closest_timestamp_ns) const¶
-
bool
findAllCloseSensorResources
(const VIMission &mission, const backend::ResourceType &type, const int64_t timestamp_ns, const int64_t tolerance_ns, std::vector<aslam::SensorId> *sensor_ids, std::vector<int64_t> *closest_timestamps_ns) const¶
-
template<typename
DataType
>
boolgetAllCloseSensorResources
(const VIMission &mission, const backend::ResourceType &type, const int64_t timestamp_ns, const int64_t tolerance_ns, std::vector<aslam::SensorId> *sensor_ids, std::vector<int64_t> *closest_timestamps_ns, std::vector<DataType> *resources) const¶
-
template<typename
DataType
>
voidaddSensorResource
(const backend::ResourceType &type, const aslam::SensorId &sensor_id, const int64_t timestamp_ns, const DataType &resource, VIMission *mission)¶
-
template<typename
DataType
>
booldeleteSensorResource
(const backend::ResourceType &type, const aslam::SensorId &sensor_id, const int64_t timestamp_ns, const bool keep_resource_file, VIMission *mission)¶
-
void
deleteAllSensorResourcesBeforeTime
(const vi_map::MissionId &mission_id, int64_t timestamp_ns, const bool delete_from_file_system)¶
-
void
deleteAllSensorResources
(const vi_map::MissionId &mission_id, const bool delete_from_file_system)¶
-
virtual bool
loadFromFolder
(const std::string &map_folder) override¶
-
bool
loadFromFolderDeprecated
(const std::string &map_folder)¶
-
virtual bool
saveToFolder
(const std::string &folder_path, const backend::SaveConfig &config) override¶
-
bool
saveToMapFolder
(const backend::SaveConfig &config)¶
-
inline bool
hasLandmarkIdInLandmarkIndex
(const vi_map::LandmarkId &id) const¶
Public Members
-
VIMap
Public Static Functions
-
static std::string
getSubFolderName
()¶
-
static bool
hasMapOnFileSystem
(const std::string &map_folder)¶
Friends
-
friend friend void serialization::deserializeMissionsAndBaseframes (const proto::VIMap &proto, vi_map::VIMap *map)
-
typedef Eigen::Matrix<unsigned char, Eigen::Dynamic, 1>