Merge pull request koide3#72 from koide3/devel

Add functions & edges for interactive SLAM

Author: koide3

.travis.yml

@@ -1,7 +1,16 @@
 sudo: required
-language: C++
+language: generic
+dist: xenial
+
 services:
   - docker
+
+matrix:
+  include:
+    - name: "Xenial kinetic"
+      env: ROS_DISTRO=kinetic
+    - name: "Bionic melodic"
+      env: ROS_DISTRO=melodic
+
 script:
-  - docker build -f docker/kinetic/Dockerfile .
-  - docker build -f docker/melodic/Dockerfile .
+  - docker build -f ./docker/$ROS_DISTRO/Dockerfile .

CMakeLists.txt

@@ -16,6 +16,7 @@ find_package(catkin REQUIRED COMPONENTS
   nmea_msgs
   sensor_msgs
   message_generation
+  interactive_markers
   ndt_omp
 )
 
@@ -58,7 +59,7 @@ generate_messages(DEPENDENCIES std_msgs)
 ###################################
 catkin_package(
   INCLUDE_DIRS include
-#  LIBRARIES hdl_scan_matching_odometry
+  LIBRARIES hdl_graph_slam_nodelet
 #  CATKIN_DEPENDS pcl_ros roscpp sensor_msgs
 #  DEPENDS system_lib
 )
@@ -104,6 +105,7 @@ add_library(hdl_graph_slam_nodelet
   src/hdl_graph_slam/map_cloud_generator.cpp
   src/hdl_graph_slam/registrations.cpp
   src/hdl_graph_slam/information_matrix_calculator.cpp
+  src/g2o/robust_kernel_io.cpp
 )
 target_link_libraries(hdl_graph_slam_nodelet
   ${catkin_LIBRARIES}
@@ -118,3 +120,4 @@ target_link_libraries(hdl_graph_slam_nodelet
   ${G2O_TYPES_SLAM3D_ADDONS}
 )
 add_dependencies(hdl_graph_slam_nodelet ${PROJECT_NAME}_gencpp)
+

README.md

@@ -236,6 +236,7 @@ roslaunch hdl_graph_slam hdl_graph_slam.launch
 Kenji Koide, Jun Miura, and Emanuele Menegatti, A Portable 3D LIDAR-based System for Long-term and Wide-area People Behavior Measurement, Advanced Robotic Systems, 2019 [[link]](https://www.researchgate.net/publication/331283709_A_Portable_3D_LIDAR-based_System_for_Long-term_and_Wide-area_People_Behavior_Measurement).
 
 ## Contact
-Kenji Koide, Active Intelligent Systems Laboratory, Toyohashi University of Technology [\[URL\]](http://www.aisl.cs.tut.ac.jp)
+Kenji Koide, [email protected]
 
-[email protected]
+Active Intelligent Systems Laboratory, Toyohashi University of Technology, Japan [\[URL\]](http://www.aisl.cs.tut.ac.jp)  
+Robot Innovation Research Center, National Institute of Advanced Industrial Science and Technology, Japan  [\[URL\]](https://unit.aist.go.jp/rirc/en/team/smart_mobility.html)

apps/hdl_graph_slam_nodelet.cpp

@@ -829,14 +829,19 @@ class HdlGraphSlamNodelet : public nodelet::Nodelet {
       std::stringstream sst;
       sst << boost::format("%s/%06d") % directory % i;
 
-      keyframes[i]->dump(sst.str());
+      keyframes[i]->save(sst.str());
     }
 
     if(zero_utm) {
       std::ofstream zero_utm_ofs(directory + "/zero_utm");
       zero_utm_ofs << *zero_utm << std::endl;
     }
 
+    std::ofstream ofs(directory + "/special_nodes.csv");
+    ofs << "anchor_node " << (anchor_node == nullptr ? -1 : anchor_node->id()) << std::endl;
+    ofs << "anchor_edge " << (anchor_edge == nullptr ? -1 : anchor_edge->id()) << std::endl;
+    ofs << "floor_node " << (floor_plane_node == nullptr ? -1 : floor_plane_node->id()) << std::endl;
+
     res.success = true;
     return true;
   }

include/g2o/edge_plane_parallel.hpp

@@ -0,0 +1,112 @@
+#ifndef EDGE_PLANE_PARALLEL_HPP
+#define EDGE_PLANE_PARALLEL_HPP
+
+#include <Eigen/Dense>
+#include <g2o/core/base_binary_edge.h>
+#include <g2o/types/slam3d_addons/vertex_plane.h>
+
+namespace g2o {
+
+class EdgePlaneParallel : public BaseBinaryEdge<3, Eigen::Vector3d, VertexPlane, VertexPlane> {
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  EdgePlaneParallel() : BaseBinaryEdge<3, Eigen::Vector3d, VertexPlane, VertexPlane>() {
+    _information.setIdentity();
+    _error.setZero();
+  }
+
+  void computeError() override {
+    const VertexPlane* v1 = static_cast<const VertexPlane*>(_vertices[0]);
+    const VertexPlane* v2 = static_cast<const VertexPlane*>(_vertices[1]);
+
+    Eigen::Vector3d normal1 = v1->estimate().normal();
+    Eigen::Vector3d normal2 = v2->estimate().normal();
+
+    if (normal1.dot(normal2) < 0.0) {
+      normal2 = -normal2;
+    }
+
+    _error = (normal2 - normal1) - _measurement;
+  }
+  virtual bool read(std::istream& is) override {
+    Eigen::Vector3d v;
+    for (int i = 0; i < 3; ++i) {
+      is >> v[i];
+    }
+
+    setMeasurement(v);
+    for (int i = 0; i < 3; ++i) {
+      for (int j = i; j < 3; ++j) {
+        is >> information()(i, j);
+        if (i != j) {
+          information()(j, i) = information()(i, j);
+        }
+      }
+    }
+
+    return true;
+  }
+
+  virtual bool write(std::ostream& os) const override {
+    for (int i = 0; i < 3; ++i) os << _measurement[i] << " ";
+    for (int i = 0; i < 3; ++i)
+      for (int j = i; j < 3; ++j) os << " " << information()(i, j);
+    return os.good();
+  }
+
+  virtual void setMeasurement(const Eigen::Vector3d& m) override { _measurement = m; }
+
+  virtual int measurementDimension() const override { return 3; }
+};
+
+class EdgePlanePerpendicular : public BaseBinaryEdge<3, Eigen::Vector3d, VertexPlane, VertexPlane> {
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  EdgePlanePerpendicular() : BaseBinaryEdge<3, Eigen::Vector3d, VertexPlane, VertexPlane>() {
+    _information.setIdentity();
+    _error.setZero();
+  }
+
+  void computeError() override {
+    const VertexPlane* v1 = static_cast<const VertexPlane*>(_vertices[0]);
+    const VertexPlane* v2 = static_cast<const VertexPlane*>(_vertices[1]);
+
+    Eigen::Vector3d normal1 = v1->estimate().normal().normalized();
+    Eigen::Vector3d normal2 = v2->estimate().normal().normalized();
+
+    _error = normal1.array() * normal2.array();
+  }
+  virtual bool read(std::istream& is) override {
+    Eigen::Vector3d v;
+    for (int i = 0; i < 3; ++i) {
+      is >> v[i];
+    }
+
+    setMeasurement(v);
+    for (int i = 0; i < 3; ++i) {
+      for (int j = i; j < 3; ++j) {
+        is >> information()(i, j);
+        if (i != j) {
+          information()(j, i) = information()(i, j);
+        }
+      }
+    }
+
+    return true;
+  }
+
+  virtual bool write(std::ostream& os) const override {
+    for (int i = 0; i < 3; ++i) os << _measurement[i] << " ";
+    for (int i = 0; i < 3; ++i)
+      for (int j = i; j < 3; ++j) os << " " << information()(i, j);
+    return os.good();
+  }
+
+  virtual void setMeasurement(const Eigen::Vector3d& m) override { _measurement = m; }
+
+  virtual int measurementDimension() const override { return 3; }
+};
+
+}  // namespace g2o
+
+#endif  // EDGE_PLANE_PARALLEL_HPP

include/g2o/robust_kernel_io.hpp

@@ -0,0 +1,16 @@
+#ifndef ROBUST_KERNEL_IO_HPP
+#define ROBUST_KERNEL_IO_HPP
+
+#include <string>
+#include <g2o/core/sparse_optimizer.h>
+
+
+namespace g2o {
+
+bool save_robust_kernels(const std::string& filename, g2o::SparseOptimizer* graph);
+
+bool load_robust_kernels(const std::string& filename, g2o::SparseOptimizer* graph);
+
+}
+
+#endif // ROBUST_KERNEL_IO_HPP

include/hdl_graph_slam/graph_slam.hpp

@@ -4,7 +4,7 @@
 #include <memory>
 #include <ros/time.h>
 
-#include <g2o/core/sparse_optimizer.h>
+#include <g2o/core/hyper_graph.h>
 
 namespace g2o {
   class VertexSE3;
@@ -17,6 +17,9 @@ namespace g2o {
   class EdgeSE3PriorXYZ;
   class EdgeSE3PriorVec;
   class EdgeSE3PriorQuat;
+  class EdgePlane;
+  class EdgePlaneParallel;
+  class EdgePlanePerpendicular;
   class RobustKernelFactory;
 }
 
@@ -25,7 +28,10 @@ namespace hdl_graph_slam {
 class GraphSLAM {
 public:
   GraphSLAM(const std::string& solver_type = "lm_var");
-  ~GraphSLAM();
+  virtual ~GraphSLAM();
+
+  int num_vertices() const;
+  int num_edges() const;
 
   /**
    * @brief add a SE3 node to the graph
@@ -93,22 +99,33 @@ class GraphSLAM {
 
   g2o::EdgeSE3PriorVec* add_se3_prior_vec_edge(g2o::VertexSE3* v_se3, const Eigen::Vector3d& direction, const Eigen::Vector3d& measurement, const Eigen::MatrixXd& information_matrix);
 
-  void add_robust_kernel(g2o::OptimizableGraph::Edge* edge, const std::string& kernel_type, double kernel_size);
+  g2o::EdgePlane* add_plane_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::Matrix3d& information);
+
+  g2o::EdgePlanePerpendicular* add_plane_perpendicular_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector3d& measurement, const Eigen::Matrix3d& information);
+
+  void add_robust_kernel(g2o::HyperGraph::Edge* edge, const std::string& kernel_type, double kernel_size);
 
   /**
    * @brief perform graph optimization
    */
-  void optimize(int num_iterations);
+  int optimize(int num_iterations);
 
   /**
-   * @brief save the pose graph
+   * @brief save the pose graph to a file
    * @param filename  output filename
    */
   void save(const std::string& filename);
 
+  /**
+   * @brief load the pose graph from file
+   * @param filename  output filename
+   */
+  bool load(const std::string& filename);
 public:
   g2o::RobustKernelFactory* robust_kernel_factory;
-  std::unique_ptr<g2o::SparseOptimizer> graph;  // g2o graph
+  std::unique_ptr<g2o::HyperGraph> graph;  // g2o graph
 };
 
 }

include/hdl_graph_slam/information_matrix_calculator.hpp

@@ -11,18 +11,33 @@ class InformationMatrixCalculator {
 public:
   using PointT = pcl::PointXYZI;
 
+  InformationMatrixCalculator(){}
   InformationMatrixCalculator(ros::NodeHandle& nh);
   ~InformationMatrixCalculator();
 
+  template<typename ParamServer>
+  void load(ParamServer& params) {
+    use_const_inf_matrix = params.template param<bool>("use_const_inf_matrix", false);
+    const_stddev_x = params.template param<double>("const_stddev_x", 0.5);
+    const_stddev_q = params.template param<double>("const_stddev_q", 0.1);
+
+    var_gain_a = params.template param<double>("var_gain_a", 20.0);
+    min_stddev_x = params.template param<double>("min_stddev_x", 0.1);
+    max_stddev_x = params.template param<double>("max_stddev_x", 5.0);
+    min_stddev_q = params.template param<double>("min_stddev_q", 0.05);
+    max_stddev_q = params.template param<double>("max_stddev_q", 0.2);
+    fitness_score_thresh = params.template param<double>("fitness_score_thresh", 0.5);
+  }
+
+  static double calc_fitness_score(const pcl::PointCloud<PointT>::ConstPtr& cloud1, const pcl::PointCloud<PointT>::ConstPtr& cloud2, const Eigen::Isometry3d& relpose, double max_range = std::numeric_limits<double>::max());
+
   Eigen::MatrixXd calc_information_matrix(const pcl::PointCloud<PointT>::ConstPtr& cloud1, const pcl::PointCloud<PointT>::ConstPtr& cloud2, const Eigen::Isometry3d& relpose) const;
 private:
   double weight(double a, double max_x, double min_y, double max_y, double x) const {
     double y = (1.0 - std::exp(-a * x)) / (1.0 - std::exp(-a * max_x));
     return min_y + (max_y - min_y) * y;
   }
 
-  double calc_fitness_score(const pcl::PointCloud<PointT>::ConstPtr& cloud1, const pcl::PointCloud<PointT>::ConstPtr& cloud2, const Eigen::Isometry3d& relpose, double max_range = std::numeric_limits<double>::max()) const;
-
 private:
   bool use_const_inf_matrix;
   double const_stddev_x;

include/hdl_graph_slam/keyframe.hpp

@@ -8,6 +8,8 @@
 
 namespace g2o {
   class VertexSE3;
+  class HyperGraph;
+  class SparseOptimizer;
 }
 
 namespace hdl_graph_slam {
@@ -22,9 +24,14 @@ struct KeyFrame {
   using Ptr = std::shared_ptr<KeyFrame>;
 
   KeyFrame(const ros::Time& stamp, const Eigen::Isometry3d& odom, double accum_distance, const pcl::PointCloud<PointT>::ConstPtr& cloud);
-  ~KeyFrame();
+  KeyFrame(const std::string& directory, g2o::HyperGraph* graph);
+  virtual ~KeyFrame();
 
-  void dump(const std::string& directory);
+  void save(const std::string& directory);
+  bool load(const std::string& directory, g2o::HyperGraph* graph);
+
+  long id() const;
+  Eigen::Isometry3d estimate() const;
 
 public:
   ros::Time stamp;                                // timestamp