Template Class CovisibilityGraph

Class Documentation

template<typename TransformationType>
class CovisibilityGraph

Manages the covisibility graph used by backend optimization.

This class receives odometry and point-observation edges from localization, converts them into GTSAM factors, runs optimization in a worker thread, and synchronizes optimized results back to the map.

Public Types

using shared_ptr = std::shared_ptr<CovisibilityGraph>

Typedef to a std::shared_ptr of type CovisibilityGraph.

using weak_ptr = std::weak_ptr<CovisibilityGraph>

Typedef to a std::weak_ptr of type CovisibilityGraph.

Public Functions

~CovisibilityGraph()

CovisibilityGraph object destructor.

void run(std::stop_token token_)

The covisibility graph module works in a separate thread.

Parameters:

token_ – Tell the thread to stop when requested.

inline size_t insert(const OdometryEdge &edge_)

Adds an odometry edge to a queue holding odometry edges.

Parameters:

edge_ – OdometryEdge with the new observation to be added.

Returns:

The size of the queue after adding the new edge.

inline size_t insert(const ObservationEdge<TransformationType> &edge_)

Adds an observation edge to a queue holding observation edges.

Parameters:

edge_ – ObservationEdge with the new observation to be added.

Returns:

The size of the queue after adding the new edge.

inline bool is_loop_queue_empty() const

Checks if the loop verification queue is empty.

Returns:

True if the loop verification queue is empty, false otherwise.

inline output_t<keyframe_id_t> next_loop_verification()

Gets the next keyframe Id from the loop verification queue.

Returns:

The next keyframe Id from the loop verification queue, or a fail if the queue is empty.

output_t<point3d_t> get_point_observation(const point_id_t pointId_, const keyframe_id_t keyframeId_ = keyframe_id_t::UNSET)

Gets a 3d point observation (in a keyframe reference frame) for a given point ID and keyframe ID. If no keyframe Id is provided, the most recent observation is returned.

Parameters:

  • pointId_ – Map point ID to query.

  • keyframeId_ – Keyframe ID to query.

Returns:

The point observation.

output_t<point2d_t> get_keypoint_observation(const point_id_t pointId_, const keyframe_id_t keyframeId_ = keyframe_id_t::UNSET)

Get a keypoint observation for a given point ID and keyframe ID. If no keyframe Id is provided, the most recent observation is returned.

Parameters:

  • pointId_ – Map point ID to query.

  • keyframeId_ – Keyframe ID to query.

Returns:

The keypoint observation.

output_t<descriptor_t> get_descriptor_observation(const point_id_t pointId_, const pose_t pose_)

Get a descriptor for a given 3D point.

Parameters:

  • pointId_ – Map point ID to query.

  • pose_ – Pose to use for the descriptor prediction.

Returns:

The descriptor observation.

output_t<descriptor_t> get_descriptor_observation(const point_id_t pointId_, const pose_t::weak_ptr posePtr_)

Get a descriptor for a given 3D point.

Parameters:

  • pointId_ – Map point ID to query.

  • posePtr_ – Pose to use for the descriptor prediction.

Returns:

The descriptor observation.

point_id_t::vector get_point_ids(const keyframe_id_t keyframeId_)

Gets Ids of all points seen by the input keyframe Id.

Parameters:

keyframeId_ – Keyframe Id to query.

Returns:

Ids of all points seen by the keyframe.

keyframe_id_t::vector get_keyframe_ids(const point_id_t pointId_)

Gets Ids of all keyframes that observe the input point Id.

Parameters:

pointId_ – Point Id to query.

Returns:

Ids of all keyframes that observe the point.

keyframe_id_t::vector get_connection_ids(const keyframe_id_t keyframeId_)

Gets the Ids of all keyframes connected to the input keyframe Id.

Parameters:

keyframeId_ – Keyframe Id to query.

Returns:

Ids of all connected keyframes.

void fit_new_descriptor(const point_id_t pointId_, const descriptor_t &descriptor_, const pose_t pose_)

Fits a new descriptor observation for a point.

Parameters:

  • pointId_ – Point Id to update the descriptor observation.

  • descriptor_ – New descriptor observation to fit into the model.

  • pose_ – Pose of the keyframe observing the point with the new descriptor. This is used to predict the expected descriptor and fit the new observation into the model.

void fit_new_descriptor(const point_id_t pointId_, const descriptor_t &descriptor_, const pose_t::weak_ptr posePtr_)

Fits a new descriptor observation for a point.

Parameters:

  • pointId_ – Point Id to update the descriptor observation.

  • descriptor_ – New descriptor observation to fit into the model.

  • posePtr_ – Pose of the keyframe observing the point with the new descriptor. This is used to predict the expected descriptor and fit the new observation into the model.

point_id_t get_up_to_date_point_id(const point_id_t pointId_)

Given a point Id, gets the most up-to-date point Id. If the point was not merged before, the input id is returned unchanged. If the point was merged, the Id of the point it was merged into is returned.

Parameters:

pointId_ – Input point Id to query. It is updated to the most up-to-date point Id.

Returns:

The most up-to-date point Id.

void close_loop(const keyframe_id_t evalKeyframeId_, const keyframe_id_t loopKeyframeId_, const pose_t evalWorldToCamera_, const point_id_t::vector &evalKeyframePointIds_, const point_id_t::vector &loopKeyframePointIds_, match_t<index_t>::vector &matches_, std::vector<bool> &inliers_)

Adds loop connection to the covisibility graph between two loop keyframes.

Parameters:

  • evalKeyframeId_ – Eval keyframe (the one we used to check for loops)

  • loopKeyframeId_ – Loop keyframe (the one already on the map)

  • evalWorldToCamera_ – Transform from world to camera for the eval keyframe

  • evalKeyframePointIds_ – Eval keyframe point Ids (synced with loopKeyframePointIds_).

  • loopKeyframePointIds_ – Loop keyframe point Ids (synced with evalKeyframePointIds_).

  • matches_ – Matches between evalKeyframePointIds_ and loopKeyframePointIds_.

  • inliers_ – List with the inliers of the matches.

Public Static Functions

static inline CovisibilityGraph::shared_ptr make_shared(const map::Map::shared_ptr map_)

Allocates a new object of type CovisibilityGraph.

Parameters:

map_ – Slam system map.

Returns:

Pointer to the new object.

Private Functions

CovisibilityGraph(const map::Map::shared_ptr &map_)

CovisibilityGraph object constructor.

Parameters:

map_ – Slam system map.

void __start_thread()

Starts the covisibility graph main threads.

inline output_t<OdometryEdge> __pop_odometry_edge()

This function gets a new odometry edge to be processed.

Returns:

New edge to be processed, or fail_t::IS_EMPTY if none are queued.

inline output_t<ObservationEdge<TransformationType>> __pop_observation_edge()

This function gets a new observation edge to be processed.

Returns:

New edge to be processed, or fail_t::IS_EMPTY if none are queued.

inline size_t __insert_to_loop_verification(const keyframe_id_t keyframeId_)

Adds a keyframe Id to the loop verification queue.

Parameters:

keyframeId_ – Id of the keyframe to be added to the loop verification.

Returns:

The size of the queue after adding the new keyframe Id.

output_t<point3d_t> __get_point_observation(const point_id_t pointId_)

Gets a 3d point observation (in a keyframe reference frame) for a given point ID.

Parameters:

pointId_ – Map point ID to query.

Returns:

The point observation.

output_t<point2d_t> __get_keypoint_observation(const point_id_t pointId_)

Gets a keypoint observation for a given point ID.

Parameters:

pointId_ – Map point ID to query.

Returns:

The keypoint observation.

void __add_keyframe_to_values(const keyframe_id_t keyframeId_, const keyframe_t::weak_ptr keyframePtr_)

Adds a new keyframe to the graph values.

Parameters:

  • keyframeId_ – ID of the keyframe to be added.

  • keyframePtr_ – Pointer to the keyframe (pose and data) as output_t.

void __add_point_to_values(const point_id_t pointId_, const point3d_t::weak_ptr pointPtr_)

Adds a new point to the graph values.

Parameters:

  • pointId_ – ID of the point to be added.

  • pointPtr_ – Pointer to the point as output_t.

bool __exists(const gtsam::Symbol symbol_)

Checks if a keyframe exists in the covisibility graph.

Parameters:

symbol_ – The GTSAM symbol representing the keyframe or point.

Returns:

True if the symbol exists in graph values, false otherwise.

void __add_odometry_edge(const OdometryEdge &edge_)

This function adds an odometry edge in the covisibility graph.

Parameters:

edge_ – The details of the odometry to be added.

void __add_observation_edge(const ObservationEdge<TransformationType> &edge_)

Adds an observation between a keyframe and a point in graph.

Parameters:

edge_ – The details of the point observation to be added.

void __add_observation_to_graph(const point_id_t pointId_, const keyframe_id_t keyframeId_, const PointObservation<TransformationType>::shared_ptr observationPtr_)

Adds a point observation to the graph.

Parameters:

  • pointId_ – Map point ID.

  • keyframeId_ – Keyframe ID.

  • observationPtr_ – Shared pointer to the PointObservation object.

void __add_uncalibrated_factors(const gtsam::Cal3_S2::shared_ptr &calibration_)

Tries to add to the graph all uncalibrated projection factors that have been created without calibration, if a valid calibration is provided.

Parameters:

calibration_ – Intrinsics to be used in the projection factors.

void __add_prior_factor(const keyframe_id_t keyframeId_, const TransformationType &pose_)

Adds a prior factor to the graph for a keyframe.

Parameters:

  • keyframeId_ – ID of the keyframe to which the prior factor is added.

  • pose_ – Pose of the keyframe to be used in the prior factor.

void __optimize(std::stop_token token_)

This function runs the optimization of the covisibility graph.

Parameters:

token_ – For stopping the thread when requested.

void __map_update(const gtsam::Values::shared_ptr optimizedValues_)

Updates the points and keyframes in the map based on the optimized values after graph optimization.

Parameters:

optimizedValues_ – The optimized values after graph optimization.

void __process_pending_points()

This function tries to update any point that failed to be updated to the map before. It uses a timeout mechanism to avoid keeping points that will never be updated.

Private Members

map::Map::shared_ptr _map

Pointer to the map created in the slam system.

OdometryEdge::queue _odometryEdges

Queue with the odometry edges created in the Localization module that will be added to the covisibility graph

ObservationEdge<TransformationType>::queue _observationEdges

Queue with the observations edges created in the Localization module that will be added to the covisibility graph

keyframe_id_t::queue _loopVerification

Queue with the loop closure candidates to be verified. It contains the Ids of the keyframes that will be evaluated for loop closure

std::jthread _edgeInsertionThread

This thread is used to insert new edges to the graph as they are created in the localization

std::jthread _optimizationThread

This thread is used to run the adjustments for the points and poses.

mutable mutex_t _mutex

Mutex for thread-safe access to the graph.