实现后端优化

lidar_localization/CMakeLists.txt

@@ -21,6 +21,7 @@ find_package(catkin REQUIRED COMPONENTS
 add_service_files(
  FILES
  saveMap.srv
+ optimizeMap.srv
 )
 
 generate_messages(
@@ -35,6 +36,7 @@ include(cmake/YAML.cmake)
 include(cmake/PCL.cmake)
 include(cmake/eigen.cmake)
 include(cmake/geographic.cmake)
+include(cmake/g2o.cmake)
 
 include_directories(include ${catkin_INCLUDE_DIRS})
 include(cmake/global_defination.cmake)
@@ -58,7 +60,7 @@ add_dependencies(front_end_node ${catkin_EXPORTED_TARGETS})
 target_link_libraries(front_end_node ${catkin_LIBRARIES} ${ALL_TARGET_LIBRARIES})
 
 add_executable(back_end_node src/apps/back_end_node.cpp ${ALL_SRCS})
-add_dependencies(back_end_node ${catkin_EXPORTED_TARGETS})
+add_dependencies(back_end_node ${catkin_EXPORTED_TARGETS} optimizeMap_gencpp)
 target_link_libraries(back_end_node ${catkin_LIBRARIES} ${ALL_TARGET_LIBRARIES})
 
 add_executable(viewer_node src/apps/viewer_node.cpp ${ALL_SRCS})

lidar_localization/cmake/g2o.cmake

@@ -0,0 +1,129 @@
+# Find the header files
+FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h
+ ${G2O_ROOT}/include
+ $ENV{G2O_ROOT}/include
+ $ENV{G2O_ROOT}
+ /usr/local/include
+ /usr/include
+ /opt/local/include
+ /sw/local/include
+ /sw/include
+ # /opt/ros/$ENV{ROS_DISTRO}/include
+ NO_DEFAULT_PATH
+ )
+
+# Macro to unify finding both the debug and release versions of the
+# libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY
+# macro.
+
+MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME)
+
+ FIND_LIBRARY("${MYLIBRARY}_DEBUG"
+ NAMES "g2o_${MYLIBRARYNAME}_d"
+ PATHS
+ ${G2O_ROOT}/lib/Debug
+ ${G2O_ROOT}/lib
+ $ENV{G2O_ROOT}/lib/Debug
+ $ENV{G2O_ROOT}/lib
+ # /opt/ros/$ENV{ROS_DISTRO}/lib
+ NO_DEFAULT_PATH
+ )
+
+ FIND_LIBRARY("${MYLIBRARY}_DEBUG"
+ NAMES "g2o_${MYLIBRARYNAME}_d"
+ PATHS
+ ~/Library/Frameworks
+ /Library/Frameworks
+ /usr/local/lib
+ /usr/local/lib64
+ /usr/lib
+ /usr/lib64
+ /opt/local/lib
+ /sw/local/lib
+ /sw/lib
+ NO_DEFAULT_PATH
+ )
+
+ FIND_LIBRARY(${MYLIBRARY}
+ NAMES "libg2o_${MYLIBRARYNAME}.so"
+ PATHS
+ ${G2O_ROOT}/lib/Release
+ ${G2O_ROOT}/lib
+ $ENV{G2O_ROOT}/lib/Release
+ $ENV{G2O_ROOT}/lib
+ # /opt/ros/$ENV{ROS_DISTRO}/lib
+ NO_DEFAULT_PATH
+ )
+
+ FIND_LIBRARY(${MYLIBRARY}
+ NAMES "libg2o_${MYLIBRARYNAME}.so"
+ PATHS
+ ~/Library/Frameworks
+ /Library/Frameworks
+ /usr/local/lib
+ /usr/local/lib64
+ /usr/lib
+ /usr/lib64
+ /opt/local/lib
+ /sw/local/lib
+ /sw/lib
+ NO_DEFAULT_PATH
+ )
+ IF(NOT ${MYLIBRARY}_DEBUG)
+ IF(MYLIBRARY)
+ SET(${MYLIBRARY}_DEBUG ${MYLIBRARY})
+ ENDIF(MYLIBRARY)
+ ENDIF( NOT ${MYLIBRARY}_DEBUG)
+
+ENDMACRO()
+
+# Find the core elements
+FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff)
+FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core)
+
+# Find the CLI library
+FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli)
+
+# Find the pluggable solvers
+FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod)
+FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse)
+FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension)
+FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense)
+FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg)
+FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear)
+FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only)
+FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen)
+
+# Find the predefined types
+FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data)
+FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp)
+FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba)
+FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d)
+FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3)
+FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d)
+FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d)
+FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D_ADDONS types_slam3d_addons)
+
+# G2O solvers declared found if we found at least one solver
+SET(G2O_SOLVERS_FOUND "NO")
+IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)
+ SET(G2O_SOLVERS_FOUND "YES")
+ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)
+
+# G2O itself declared found if we found the core libraries and at least one solver
+SET(G2O_FOUND "NO")
+IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
+ SET(G2O_FOUND "YES")
+ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
+
+# 加入引用文件和库文件中去
+include_directories(SYSTEM ${G2O_INCLUDE_DIR})
+list(APPEND ALL_TARGET_LIBRARIES 
+ ${G2O_TYPES_DATA}
+ ${G2O_CORE_LIBRARY}
+ ${G2O_STUFF_LIBRARY}
+ ${G2O_SOLVER_PCG}
+ ${G2O_SOLVER_CSPARSE}
+ ${G2O_SOLVER_CHOLMOD}
+ ${G2O_TYPES_SLAM3D}
+ ${G2O_TYPES_SLAM3D_ADDONS})

lidar_localization/config/back_end/config.yaml

@@ -4,7 +4,16 @@ data_path: ./ # 数据存放路径
 key_frame_distance: 2.0 # 关键帧距离
 
 # 优化
-optimize_step_with_none: 100 # 没有其他信息时，每隔 step 发送一次优化的位姿
-optimize_step_with_gnss: 100 # 每累计 step 个 gnss 观测时，优化一次
-optimize_step_with_loop: 10 # 每累计 step 个闭环约束时优化一次
+graph_optimizer_type: g2o # 图优化库，目前支持g2o
 
+use_gnss: true
+use_loop_close: false
+
+optimize_step_with_key_frame: 10000 # 没有其他信息时，每隔 step 发送一次优化的位姿
+optimize_step_with_gnss: 950 # 每累计 step 个 gnss 观测时，优化一次
+optimize_step_with_loop: 100 # 每累计 step 个闭环约束时优化一次
+
+g2o_param:
+ odom_edge_noise: [0.5, 0.5, 0.5, 0.001, 0.001, 0.001] # 噪声：x y z yaw roll pitch
+ close_loop_noise: [2.0, 2.0, 2.0, 0.01, 0.01, 0.01] # 噪声：x y z yaw roll pitch
+ gnss_noise: [2.0, 2.0, 2.0] # 噪声：x y z

lidar_localization/include/lidar_localization/mapping/back_end/back_end.hpp

@@ -15,13 +15,16 @@
 #include "lidar_localization/sensor_data/pose_data.hpp"
 #include "lidar_localization/sensor_data/key_frame.hpp"
 
+#include "lidar_localization/models/graph_optimizer/g2o/g2o_graph_optimizer.hpp"
+
 namespace lidar_localization {
 class BackEnd {
  public:
  BackEnd();
 
  bool Update(const CloudData& cloud_data, const PoseData& laser_odom, const PoseData& gnss_pose);
 
+ bool ForceOptimize();
  void GetOptimizedKeyFrames(std::deque<KeyFrame>& key_frames_deque);
  bool HasNewKeyFrame();
  bool HasNewOptimized();
@@ -30,10 +33,12 @@ class BackEnd {
  private:
  bool InitWithConfig();
  bool InitParam(const YAML::Node& config_node);
+ bool InitGraphOptimizer(const YAML::Node& config_node);
  bool InitDataPath(const YAML::Node& config_node);
 
  void ResetParam();
  bool SaveTrajectory(const PoseData& laser_odom, const PoseData& gnss_pose);
+ bool AddNodeAndEdge(const PoseData& gnss_data);
  bool MaybeNewKeyFrame(const CloudData& cloud_data, const PoseData& laser_odom);
  bool MaybeOptimized();
 
@@ -45,16 +50,41 @@ class BackEnd {
  std::ofstream laser_odom_ofs_;
 
  float key_frame_distance_ = 2.0;
- int optimize_step_with_none_ = 100;
- int optimize_step_with_gnss_ = 100;
- int optimize_step_with_loop_ = 10;
 
  bool has_new_key_frame_ = false;
  bool has_new_optimized_ = false;
 
- Eigen::Matrix4f last_key_pose_ = Eigen::Matrix4f::Identity();
- KeyFrame latest_key_frame_;
+ KeyFrame current_key_frame_;
  std::deque<KeyFrame> key_frames_deque_;
+
+ // 优化器
+ std::shared_ptr<InterfaceGraphOptimizer> graph_optimizer_ptr_;
+
+ class GraphOptimizerConfig {
+ public:
+ GraphOptimizerConfig() {
+ odom_edge_noise.resize(6);
+ close_loop_noise.resize(6);
+ gnss_noise.resize(3);
+ }
+
+ public:
+ bool use_gnss = true;
+ bool use_loop_close = false;
+
+ Eigen::VectorXd odom_edge_noise;
+ Eigen::VectorXd close_loop_noise;
+ Eigen::VectorXd gnss_noise;
+
+ int optimize_step_with_key_frame = 100;
+ int optimize_step_with_gnss = 100;
+ int optimize_step_with_loop = 10;
+ };
+ GraphOptimizerConfig graph_optimizer_config_;
+
+ int new_gnss_cnt_ = 0;
+ int new_loop_cnt_ = 0;
+ int new_key_frame_cnt_ = 0;
 };
 }
 

lidar_localization/include/lidar_localization/mapping/back_end/back_end_flow.hpp

@@ -24,6 +24,8 @@ class BackEndFlow {
 
  bool Run();
 
+ bool ForceOptimize();
+
  private:
  bool ReadData();
  bool HasData();

lidar_localization/include/lidar_localization/mapping/viewer/viewer.hpp

@@ -20,10 +20,10 @@ class Viewer {
  public:
  Viewer();
 
- bool Update(std::deque<KeyFrame>& new_key_frames,
-  std::deque<KeyFrame>& optimized_key_frames,
- PoseData transformed_data,
- CloudData cloud_data);
+ bool UpdateWithOptimizedKeyFrames(std::deque<KeyFrame>& optimized_key_frames);
+ bool UpdateWithNewKeyFrame(std::deque<KeyFrame>& new_key_frames,
+  PoseData transformed_data,
+  CloudData cloud_data);
 
  bool SaveMap();
  Eigen::Matrix4f& GetCurrentPose();
@@ -41,7 +41,6 @@ class Viewer {
  std::shared_ptr<CloudFilterInterface>& filter_ptr, 
  const YAML::Node& config_node);
 
- void ResetParam();
  bool OptimizeKeyFrames();
  bool JointGlobalMap(CloudData::CLOUD_PTR& global_map_ptr);
  bool JointLocalMap(CloudData::CLOUD_PTR& local_map_ptr);

lidar_localization/include/lidar_localization/mapping/viewer/viewer_flow.hpp

@@ -31,8 +31,8 @@ class ViewerFlow {
  bool ReadData();
  bool HasData();
  bool ValidData();
- bool UpdateViewer();
- bool PublishData();
+ bool PublishGlobalData();
+ bool PublishLocalData();
 
  private:
  // subscriber

lidar_localization/include/lidar_localization/models/graph_optimizer/g2o/edge/edge_se3_priorquat.hpp

@@ -0,0 +1,61 @@
+/*
+ * @Description: 姿态的先验边
+ * @Author: Ren Qian
+ * @Date: 2020-03-01 18:05:35
+ */
+#ifndef LIDAR_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_G2O_EDGE_EDGE_SE3_PRIORQUAT_HPP_
+#define LIDAR_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_G2O_EDGE_EDGE_SE3_PRIORQUAT_HPP_
+
+#include <g2o/types/slam3d/types_slam3d.h>
+#include <g2o/types/slam3d_addons/types_slam3d_addons.h>
+
+namespace g2o {
+class EdgeSE3PriorQuat : public g2o::BaseUnaryEdge<3, Eigen::Quaterniond, g2o::VertexSE3> {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ EdgeSE3PriorQuat()
+ :g2o::BaseUnaryEdge<3, Eigen::Quaterniond, g2o::VertexSE3>() {
+ }
+
+ void computeError() override {
+ const g2o::VertexSE3* v1 = static_cast<const g2o::VertexSE3*>(_vertices[0]);
+
+ Eigen::Quaterniond estimate = Eigen::Quaterniond(v1->estimate().linear());
+ if(estimate.w() < 0) {
+ estimate.coeffs() = -estimate.coeffs();
+ }
+ _error = estimate.vec() - _measurement.vec();
+ }
+
+ void setMeasurement(const Eigen::Quaterniond& m) override {
+ _measurement = m;
+ if(m.w() < 0.0) {
+ _measurement.coeffs() = -m.coeffs();
+ }
+ }
+
+ virtual bool read(std::istream& is) override {
+ Eigen::Quaterniond q;
+ is >> q.w() >> q.x() >> q.y() >> q.z();
+ setMeasurement(q);
+ for (int i = 0; i < information().rows(); ++i)
+ for (int j = i; j < information().cols(); ++j) {
+ is >> information()(i, j);
+ if (i != j)
+ information()(j, i) = information()(i, j);
+ }
+ return true;
+ }
+
+ virtual bool write(std::ostream& os) const override {
+ Eigen::Quaterniond q = _measurement;
+ os << q.w() << " " << q.x() << " " << q.y() << " " << q.z();
+ for (int i = 0; i < information().rows(); ++i)
+ for (int j = i; j < information().cols(); ++j)
+ os << " " << information()(i, j);
+ return os.good();
+ }
+};
+}
+
+#endif