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>
void getAllIdsInMission(const MissionId &mission, std::vector<IdType> *all_ids_of_type) const¶
-
template<typename IdType>
void getMissionIds(const IdType &object_id, vi_map::MissionIdSet *mission_ids) const¶
-
template<typename IdType>
void getMissionIds(const std::vector<IdType> &object_ids, vi_map::MissionIdSet *mission_ids) const¶
-
template<typename IdType>
void getMissionIds(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::EdgeType EdgeType>
size_t mergeEdgesOfNeighboringVertices(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>
bool getMissionResource(const backend::ResourceType &type, const MissionIdList &involved_mission_ids, DataType *resource) const¶
-
template<typename DataType>
void storeMissionResource(const backend::ResourceType &type, const DataType &resource, const MissionIdList &involved_mission_ids)¶
-
template<typename DataType>
void deleteMissionResource(const backend::ResourceType &resource_type, const MissionIdList &involved_mission_ids)¶
-
template<typename DataType>
void replaceMissionResource(const backend::ResourceType &resource_type, const DataType &resource, const MissionIdList &involved_mission_ids)¶
-
template<typename DataType>
void storeFrameResourceToFolder(const DataType &resource, const std::string &resource_folder, const unsigned int frame_idx, const backend::ResourceType &type, Vertex *vertex_ptr)¶
-
template<typename DataType>
void storeFrameResource(const DataType &resource, const unsigned int frame_idx, const backend::ResourceType &type, Vertex *vertex_ptr)¶
-
template<typename DataType>
bool getFrameResource(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>
void replaceFrameResource(const DataType &resource, const unsigned int frame_idx, const backend::ResourceType &type, Vertex *vertex_ptr)¶
-
template<typename DataType>
void deleteFrameResourcesOfType(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>
bool getSensorResource(const VIMission &mission, const backend::ResourceType &type, const aslam::SensorId &sensor_id, const int64_t timestamp_ns, DataType *resource) const¶
-
template<typename DataType>
bool getClosestSensorResource(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>
bool getAllCloseSensorResources(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>
void addSensorResource(const backend::ResourceType &type, const aslam::SensorId &sensor_id, const int64_t timestamp_ns, const DataType &resource, VIMission *mission)¶
-
template<typename DataType>
bool deleteSensorResource(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
-
typedef Eigen::Matrix<unsigned char, Eigen::Dynamic, 1> DescriptorType¶