Class GraphSLAM

Class Documentation

class GraphSLAM

Public Functions

GraphSLAM(const std::string &solver_type = "lm_var_cholmod", bool save_time = false)

Constructor for class GraphSLAM.

Parameters:

solver_type – Default value is lm_var

virtual ~GraphSLAM()
int retrieve_total_nbr_of_vertices() const

Counts the number of vertices in the graph.

Returns:

Number of vertices in the graph.

int retrieve_total_nbr_of_edges() const

Counts the number of edges in the graph.

Returns:

Number of edges in the graph.

int retrieve_local_nbr_of_vertices() const

Counts the number of vertices in the graph that are local.

Returns:

Number of vertices in the graph that are local.

int retrieve_local_nbr_of_edges() const

Counts the number of edges in the graph that are local.

Returns:

Number of edges in the graph that are local.

int increment_local_nbr_of_vertices()
Returns:

int increment_local_nbr_of_edges()
Returns:

void select_solver_type(const std::string &solver_type)

Set the current solver type.

Parameters:

solver_type –

g2o::VertexSE3 *add_se3_node(const Eigen::Isometry3d &pose, bool use_vertex_size_id = false)

Add a SE3 node to the graph.

Parameters:

pose –

Returns:

Registered node

g2o::VertexSE3 *copy_se3_node(const g2o::VertexSE3 *node)

copy an SE3 node from another graph.

Parameters:

node –

Returns:

Registered node

g2o::VertexPlane *add_plane_node(const Eigen::Vector4d &plane_coeffs)

Add a plane node to the graph.

Parameters:

plane_coeffs –

Returns:

Registered node

g2o::VertexPlane *add_plane_node(const Eigen::Vector4d &plane_coeffs, const int id)

Add a plane node to the graph.

Parameters:

  • plane_coeffs –

  • id –

Returns:

Registered node

g2o::VertexPlane *copy_plane_node(const g2o::VertexPlane *node)

copy a plane node from another graph

Parameters:

node –

Returns:

Registered node

bool remove_plane_node(g2o::VertexPlane *plane_vertex)

remove a plane node from the graph

Parameters:

plane – vertex

Returns:

success or failure

bool remove_room_node(g2o::VertexRoom *room_vertex)

remove a room node from the graph

Parameters:

room – vertex

Returns:

success or failure

g2o::VertexPointXYZ *add_point_xyz_node(const Eigen::Vector3d &xyz)

add a point_xyz node to the graph

Parameters:

xyz –

Returns:

Registered node

g2o::VertexRoom *add_room_node(const Eigen::Isometry3d &room_pose)

Add a room node to the graph.

Parameters:

room_pose –

Returns:

Registered node

g2o::VertexDoorWay *add_doorway_node(const Eigen::Isometry3d &doorway_pose)

Add a Doorway node to the graph.

Parameters:

doorway_pose –

Returns:

Registered node

g2o::VertexRoom *copy_room_node(const g2o::VertexRoom *node)

copy a room node from another graph

Parameters:

node –

Returns:

Registered node

g2o::VertexFloor *add_floor_node(const Eigen::Isometry3d &floor_pose)

Add a floor node to the graph.

Parameters:

floor_pose –

Returns:

Registered node

g2o::VertexFloor *copy_floor_node(const g2o::VertexFloor *node)

copy a floor node from another

Parameters:

node –

Returns:

Registered node

void update_floor_node(g2o::VertexFloor *floor_node, const Eigen::Isometry3d &floor_pose)

Update the floor node estimate in the graph.

Parameters:

floor_node – @para floor_pose

Returns:

Registered node

g2o::VertexWallXYZ *add_wall_node(const Eigen::Vector3d &wall_center)

add a Wall node to the graph

Parameters:

wall_center –

Returns:

registered node

g2o::VertexWallXYZ *copy_wall_node(const g2o::VertexWallXYZ *wall_node)

copy a Wall node to the graph

Parameters:

wall_node –

Returns:

registered node

g2o::VertexDeviation *add_deviation_node(const Eigen::Isometry3d &pose)

Add a SE3 Deviation node to the graph.

Parameters:

pose –

Returns:

Registered node

g2o::EdgeSE3 *add_se3_edge(g2o::VertexSE3 *v1, g2o::VertexSE3 *v2, const Eigen::Isometry3d &relative_pose, const Eigen::MatrixXd &information_matrix, const bool use_edge_size_id = false)

Add an edge between SE3 nodes.

Parameters:

  • v1 – node1

  • v2 – node2

  • relative_pose – relative pose between node1 and node2

  • information_matrix – information matrix (it must be 6x6)

Returns:

registered edge

g2o::EdgeLoopClosure *add_loop_closure_edge(g2o::VertexSE3 *v1, g2o::VertexSE3 *v2, const Eigen::Isometry3d &relative_pose, const Eigen::MatrixXd &information_matrix)

Add loop closure edge between SE3 nodes.

Parameters:

  • v1 – node1

  • v2 – node2

  • relative_pose – relative pose between node1 and node2

  • information_matrix – information matrix (it must be 6x6)

Returns:

registered edge

g2o::EdgeSE3 *copy_se3_edge(g2o::EdgeSE3 *e, g2o::VertexSE3 *v1, g2o::VertexSE3 *v2)

copy an edge from another graph

Parameters:

e – edge

Returns:

registered edge

g2o::EdgeSE3 *copy_loop_closure_edge(g2o::EdgeLoopClosure *e, g2o::VertexSE3 *v1, g2o::VertexSE3 *v2)

copy a loop closure edge from another graph

Parameters:

e – edge

Returns:

registered edge

g2o::EdgeSE3Plane *add_se3_plane_edge(g2o::VertexSE3 *v_se3, g2o::VertexPlane *v_plane, const Eigen::Vector4d &plane_coeffs, const Eigen::MatrixXd &information_matrix)

Add an edge between an SE3 node and a plane node.

Parameters:

  • v_se3 – SE3 node

  • v_plane – plane node

  • plane_coeffs – plane coefficients w.r.t. v_se3

  • information_matrix – information matrix (it must be 3x3)

Returns:

registered edge

g2o::EdgeSE3Plane *copy_se3_plane_edge(g2o::EdgeSE3Plane *e, g2o::VertexSE3 *v1, g2o::VertexPlane *v2)

copy an edge from another graph

Parameters:

  • e – SE3plane edge

  • v1 – se3 node

  • v2 – plane node

Returns:

registered edge

bool remove_se3_plane_edge(g2o::EdgeSE3Plane *se3_plane_edge)

Remove a plane edge from the graph.

Parameters:

se3_plane_edge –

Returns:

Succes or failure

void update_se3edge_information(g2o::EdgeSE3 *edge_se3, Eigen::MatrixXd information_matrix)

Update the information of an se3 edge.

Parameters:

edge_se3 –

g2o::EdgeSE3PointToPlane *add_se3_point_to_plane_edge(g2o::VertexSE3 *v_se3, g2o::VertexPlane *v_plane, const Eigen::Matrix4d &points_matrix, const Eigen::MatrixXd &information_matrix)

Add an edge between an SE3 node and to a plane using point to plane distances.

Parameters:

  • v_se3 – SE3 node

  • v_plane – plane node

  • points_matrix – plane coefficients w.r.t. v_se3

  • information_matrix – information matrix (it must be 1x1)

Returns:

registered edge

g2o::EdgeSE3PointXYZ *add_se3_point_xyz_edge(g2o::VertexSE3 *v_se3, g2o::VertexPointXYZ *v_xyz, const Eigen::Vector3d &xyz, const Eigen::MatrixXd &information_matrix)

Add an edge between an SE3 node and a point_xyz node.

Parameters:

  • v_se3 – SE3 node

  • v_xyz – point_xyz node

  • xyz – xyz coordinate

  • information – information_matrix (it must be 3x3)

Returns:

registered edge

g2o::EdgePlanePriorNormal *add_plane_normal_prior_edge(g2o::VertexPlane *v, const Eigen::Vector3d &normal, const Eigen::MatrixXd &information_matrix)

Add a prior edge to an SE3 node.

Parameters:

  • v – Plane

  • normal –

  • information_matrix –

Returns:

registered edge

g2o::EdgePlanePriorDistance *add_plane_distance_prior_edge(g2o::VertexPlane *v, double distance, const Eigen::MatrixXd &information_matrix)
Parameters:

  • v – Plane

  • distance –

  • information_matrix –

Returns:

registered edge

g2o::EdgeSE3PriorXY *add_se3_prior_xy_edge(g2o::VertexSE3 *v_se3, const Eigen::Vector2d &xy, const Eigen::MatrixXd &information_matrix)
Parameters:

  • v_se3 – Node

  • xy –

  • information_matrix –

Returns:

registered edge

g2o::EdgeSE3PriorXYZ *add_se3_prior_xyz_edge(g2o::VertexSE3 *v_se3, const Eigen::Vector3d &xyz, const Eigen::MatrixXd &information_matrix)
Parameters:

  • v_se3 – Node

  • xyz –

  • information_matrix –

Returns:

registered edge

g2o::EdgeSE3PriorQuat *add_se3_prior_quat_edge(g2o::VertexSE3 *v_se3, const Eigen::Quaterniond &quat, const Eigen::MatrixXd &information_matrix)
Parameters:

  • v_se3 – node

  • quat – Quarternion

  • information_matrix –

Returns:

registered edge

g2o::EdgeSE3PriorVec *add_se3_prior_vec_edge(g2o::VertexSE3 *v_se3, const Eigen::Vector3d &direction, const Eigen::Vector3d &measurement, const Eigen::MatrixXd &information_matrix)
Parameters:

  • v_se3 –

  • direction –

  • measurement –

  • information_matrix –

Returns:

registered edge

g2o::EdgePlane *add_plane_edge(g2o::VertexPlane *v_plane1, g2o::VertexPlane *v_plane2, const Eigen::Vector4d &measurement, const Eigen::Matrix4d &information)
Parameters:

  • v_plane1 –

  • v_plane2 –

  • measurement –

  • information –

Returns:

registered edge

g2o::EdgePlaneIdentity *add_plane_identity_edge(g2o::VertexPlane *v_plane1, g2o::VertexPlane *v_plane2, const Eigen::Vector4d &measurement, const Eigen::Matrix4d &information)
Parameters:

  • v_plane1 –

  • v_plane2 –

  • measurement –

  • information –

Returns:

registered edge

g2o::EdgePlaneParallel *add_plane_parallel_edge(g2o::VertexPlane *v_plane1, g2o::VertexPlane *v_plane2, const Eigen::Vector3d &measurement, const Eigen::MatrixXd &information)
Parameters:

  • v_plane1 –

  • v_plane2 –

  • measurement –

  • information –

Returns:

registered edge

g2o::EdgePlanePerpendicular *add_plane_perpendicular_edge(g2o::VertexPlane *v_plane1, g2o::VertexPlane *v_plane2, const Eigen::Vector3d &measurement, const Eigen::MatrixXd &information)
Parameters:

  • v_plane1 –

  • v_plane2 –

  • measurement –

  • information –

Returns:

registered edge

g2o::Edge2Planes *add_2planes_edge(g2o::VertexPlane *v_plane1, g2o::VertexPlane *v_plane2, const Eigen::MatrixXd &information)

add edges between duplicate planes

Parameters:

  • v_plane1 –

  • v_plane2 –

  • measurement –

  • information –

Returns:

registered edge

g2o::Edge2Planes *copy_2planes_edge(g2o::Edge2Planes *e, g2o::VertexPlane *v1, g2o::VertexPlane *v2)

copy the 2planes edges from another graph

Parameters:

  • e – 2planes edge

  • v1 – plane1 edge

  • v2 – plane2 edge

Returns:

registered edge

g2o::EdgeWall2Planes *copy_wall_2planes_edge(g2o::EdgeWall2Planes *e, g2o::VertexWallXYZ *v1, g2o::VertexPlane *v2, g2o::VertexPlane *v3)
Parameters:

  • e –

  • v2 –

  • v3 –

Returns:

g2o::EdgeWall2Planes*

g2o::EdgeSE3PlanePlane *add_se3_point_to_2planes_edge(g2o::VertexDeviation *v_se3, g2o::VertexPlane *v_plane1, g2o::VertexPlane *v_plane2, const Eigen::MatrixXd &information)

Deviation connection edge between two planes.

Parameters:

  • v_se3 – Deviation vertex

  • v1 – plane1 edge

  • v2 – plane2 edge

Returns:

registered edge

g2o::EdgeSE3Room *add_se3_room_edge(g2o::VertexSE3 *v_se3, g2o::VertexRoom *v_room, const Eigen::Vector2d &measurement, const Eigen::MatrixXd &information)
Parameters:

  • v_se3 –

  • v_room –

  • measurement –

  • information –

Returns:

registered edge

g2o::EdgeRoom2Planes *add_room_2planes_edge(g2o::VertexRoom *v_room, g2o::VertexPlane *v_plane1, g2o::VertexPlane *v_plane2, g2o::VertexRoom *v_cluster_center, const Eigen::MatrixXd &information)
Parameters:

  • v_room –

  • v_plane1 –

  • v_plane2 –

  • v_cluster_center –

  • information –

Returns:

registered edge

bool remove_room_2planes_edge(g2o::EdgeRoom2Planes *room_plane_edge)
g2o::EdgeRoom2Planes *copy_room_2planes_edge(g2o::EdgeRoom2Planes *e, g2o::VertexRoom *v1, g2o::VertexPlane *v2, g2o::VertexPlane *v3, g2o::VertexRoom *v4)
Parameters:

  • edge – edgeroom2plane

  • v1 – vertex room

  • v2 – vertex plane1

  • v3 – vertex plane2

Returns:

registered edge

g2o::EdgeRoom4Planes *add_room_4planes_edge(g2o::VertexRoom *v_room, g2o::VertexPlane *v_xplane1, g2o::VertexPlane *v_xplane2, g2o::VertexPlane *v_yplane1, g2o::VertexPlane *v_yplane2, const Eigen::MatrixXd &information)
Parameters:

  • v_room –

  • v_plane1 –

  • v_plane2 –

  • v_yplane1 –

  • v_yplane2 –

  • information –

Returns:

registered edge

g2o::EdgeRoom4Planes *copy_room_4planes_edge(g2o::EdgeRoom4Planes *e, g2o::VertexRoom *v1, g2o::VertexPlane *v2, g2o::VertexPlane *v3, g2o::VertexPlane *v4, g2o::VertexPlane *v5)
Parameters:

  • edge – edgeroom2plane

  • v1 – vertex room

  • v2 – vertex xplane1

  • v3 – vertex xplane2

  • v4 – vertex yplane1

  • v5 – vertex yplane2

Returns:

registered edge

g2o::EdgeFloorRoom *add_floor_room_edge(g2o::VertexFloor *v_floor, g2o::VertexRoom *v_room, const Eigen::Vector2d &measurement, const Eigen::MatrixXd &information)

add edge between floor and its rooms

Parameters:

  • v_floor –

  • v_room –

  • measurement –

  • information –

Returns:

registered edge

g2o::EdgeFloorRoom *copy_floor_room_edge(g2o::EdgeFloorRoom *e, g2o::VertexFloor *v1, g2o::VertexRoom *v2)

copy the floor room edge from a graph

Parameters:

  • e – edge floor room

  • v1 – vertex floor

  • v2 – vertex room

Returns:

registered edge

bool remove_room_room_edge(g2o::EdgeFloorRoom *room_room_edge)
Parameters:

room_room_edge –

Returns:

Succes or failure

g2o::EdgeXInfiniteRoomXInfiniteRoom *add_x_infinite_room_x_infinite_room_edge(g2o::VertexInfiniteRoom *v_xcorr1, g2o::VertexInfiniteRoom *v_xcorr2, const double &measurement, const Eigen::MatrixXd &information)
Parameters:

  • v_xcorr1 –

  • v_xcorr2 –

  • measurement –

  • information –

Returns:

registered edge

g2o::EdgeYInfiniteRoomYInfiniteRoom *add_y_infinite_room_y_infinite_room_edge(g2o::VertexInfiniteRoom *v_ycorr1, g2o::VertexInfiniteRoom *v_ycorr2, const double &measurement, const Eigen::MatrixXd &information)
Parameters:

  • v_ycorr1 –

  • v_ycorr2 –

  • measurement –

  • information –

Returns:

registered edge

g2o::EdgeWall2Planes *add_wall_2planes_edge(g2o::VertexWallXYZ *v_wall, g2o::VertexPlane *v_plane1, g2o::VertexPlane *v_plane2, Eigen::Vector3d wall_point, const Eigen::MatrixXd &information)
g2o::EdgeDoorWay2Rooms *add_doorway_2rooms_edge(g2o::VertexDoorWay *v_door_r1, g2o::VertexDoorWay *v_door_r2, g2o::VertexRoom *v_room1, g2o::VertexRoom *v_room2, const Eigen::MatrixXd &information)
Parameters:

  • v_door_r1 –

  • v_door_r2 –

  • v_room1 –

  • v_room2 –

  • information –

Returns:

registered edge

g2o::EdgeSE3RoomRoom *add_deviation_two_rooms_edge(g2o::VertexDeviation *v1, g2o::VertexRoom *v2, g2o::VertexRoom *v3, const Eigen::MatrixXd &information)

deviations betwen rooms edge

Parameters:

  • v1 – vertex deviation

  • v2 – vertex room

  • v3 – vertex room

Returns:

registered edge

g2o::Edge2Rooms *add_2rooms_edge(g2o::VertexRoom *v1, g2o::VertexRoom *v2, const Eigen::MatrixXd &information)

Merge prior and online rooms with 0 error.

Parameters:

  • v1 – vertex room 1

  • v2 – vertex room 2

Returns:

registered edge

void add_robust_kernel(g2o::HyperGraph::Edge *edge, const std::string &kernel_type, double kernel_size)
Parameters:

  • edge –

  • kernel_type –

  • kernel_size –

int optimize(const std::string optimization_type = " ", const int num_iterations = 512)

Perform graph optimization.

Parameters:

num_iterations –

Returns:

bool compute_landmark_marginals(g2o::SparseBlockMatrix<Eigen::MatrixXd> &spinv, std::vector<std::pair<int, int>> vert_pairs_vec)
Parameters:

  • spinv –

  • vert_pairs_vec –

Returns:

Success or failure

void save(const std::string &filename)

Save the pose graph to a file.

Parameters:

filename – output filename

bool load(const std::string &filename)

Load the pose graph from file.

Parameters:

filename – output filename

Public Members

g2o::RobustKernelFactory *robust_kernel_factory
std::unique_ptr<g2o::SparseOptimizer> graph
int nbr_of_vertices
int nbr_of_edges
int timing_counter
double sum_prev_timings
bool save_compute_time
std::ofstream time_recorder