Program Listing for File graph_slam.hpp

↰ Return to documentation for file (include/s_graphs/backend/graph_slam.hpp)

/*
Copyright (c) 2023, University of Luxembourg
All rights reserved.

Redistributions and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this
   list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.

3. Neither the name of the copyright holder nor the names of its
   contributors may be used to endorse or promote products derived from
   this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS'
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
*/

#ifndef GRAPH_SLAM_HPP
#define GRAPH_SLAM_HPP

#include <g2o/core/hyper_graph.h>
#include <g2o/core/sparse_block_matrix.h>
#include <g2o/core/sparse_optimizer.h>

#include <g2o/edge_doorway_two_rooms.hpp>
#include <g2o/edge_se3_two_planes.hpp>
#include <g2o/edge_se3_two_rooms.hpp>
#include <g2o/edge_wall_two_planes.hpp>
#include <g2o/vertex_deviation.hpp>
#include <g2o/vertex_wall.hpp>
#include <memory>

#include "rclcpp/rclcpp.hpp"
namespace g2o {
class VertexSE3;
class VertexPlane;
class VertexPointXYZ;
class VertexInfiniteRoom;
class EdgeSE3;
class EdgeLoopClosure;
class EdgeSE3Plane;
class EdgeSE3PointToPlane;
class EdgeSE3PointXYZ;
class EdgeSE3PriorXY;
class EdgeSE3PriorXYZ;
class EdgeSE3PriorVec;
class EdgeSE3PriorQuat;
class EdgePlane;
class EdgePlaneIdentity;
class EdgePlaneParallel;
class EdgeSE3InfiniteRoom;
class EdgeInfiniteRoomXPlane;
class EdgeInfiniteRoomYPlane;
class EdgeSE3Room;
class EdgeRoom2Planes;
class EdgeRoom4Planes;
class EdgeFloorRoom;
class Edge2Rooms;
class EdgeXInfiniteRoomXInfiniteRoom;
class EdgeYInfiniteRoomYInfiniteRoom;
class EdgePlanePerpendicular;
class Edge2Planes;
class EdgeSE3PlanePlane;
class EdgeSE3RoomRoom;
class EdgePlanePriorNormal;
class EdgePlanePriorDistance;
class EdgeDoorWay2Rooms;
class RobustKernelFactory;
class VertexRoom;
class VertexFloor;
class VertexDoorWay;
class VertexDeviation;
}  // namespace g2o

namespace s_graphs {

class GraphSLAM {
 public:
  GraphSLAM(const std::string& solver_type = "lm_var_cholmod", bool save_time = false);
  virtual ~GraphSLAM();

  int retrieve_total_nbr_of_vertices() const;

  int retrieve_total_nbr_of_edges() const;

  int retrieve_local_nbr_of_vertices() const;

  int retrieve_local_nbr_of_edges() const;

  int increment_local_nbr_of_vertices();

  int increment_local_nbr_of_edges();

  void select_solver_type(const std::string& solver_type);

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

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

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

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

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

  bool remove_plane_node(g2o::VertexPlane* plane_vertex);

  bool remove_room_node(g2o::VertexRoom* room_vertex);

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

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

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

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

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

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

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

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

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

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

  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);

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

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

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

  g2o::EdgeSE3Plane* add_se3_plane_edge(g2o::VertexSE3* v_se3,
                                        g2o::VertexPlane* v_plane,
                                        const Eigen::Vector4d& plane_coeffs,
                                        const Eigen::MatrixXd& information_matrix);
  g2o::EdgeSE3Plane* copy_se3_plane_edge(g2o::EdgeSE3Plane* e,
                                         g2o::VertexSE3* v1,
                                         g2o::VertexPlane* v2);

  bool remove_se3_plane_edge(g2o::EdgeSE3Plane* se3_plane_edge);

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

  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);

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  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);

  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);

  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);

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

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

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

  bool remove_room_room_edge(g2o::EdgeFloorRoom* room_room_edge);

  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);

  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);

  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);

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

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

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

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

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

  void save(const std::string& filename);

  bool load(const std::string& filename);

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

}  // namespace s_graphs

#endif  // GRAPH_SLAM_HPP