fix orientation constraint bug & make solver configurable

koide3 · Oct 10, 2019 · 938e6c0 · 938e6c0
1 parent e5f8b27
commit 938e6c0
Show file tree

Hide file tree

Showing 6 changed files with 30 additions and 296 deletions.
diff --git a/.gitignore b/.gitignore
@@ -0,0 +1,3 @@
+rviz/*
+launch/*
+imgui.ini
diff --git a/include/g2o/edge_se3_priorquat.hpp b/include/g2o/edge_se3_priorquat.hpp
@@ -16,7 +16,8 @@ class EdgeSE3PriorQuat : public g2o::BaseUnaryEdge<3, Eigen::Quaterniond, g2o::V
 			const g2o::VertexSE3* v1 = static_cast<const g2o::VertexSE3*>(_vertices[0]);
 
 			Eigen::Quaterniond estimate = Eigen::Quaterniond(v1->estimate().linear());
-			if(estimate.w() < 0) {
+
+			if(Eigen::Quaterniond(v1->estimate().linear()).coeffs().dot(estimate.coeffs()) < 0.0) {
 				estimate.coeffs() = -estimate.coeffs();
 			}
 			_error = estimate.vec() - _measurement.vec();

diff --git a/include/hdl_graph_slam/graph_slam.hpp b/include/hdl_graph_slam/graph_slam.hpp
@@ -35,6 +35,8 @@ class GraphSLAM {
   int num_vertices() const;
   int num_edges() const;
 
+  void set_solver(const std::string& solver_type);
+
   /**
    * @brief add a SE3 node to the graph
    * @param pose

diff --git a/include/hdl_graph_slam/information_matrix_calculator.hpp b/include/hdl_graph_slam/information_matrix_calculator.hpp
@@ -26,7 +26,7 @@ class InformationMatrixCalculator {
     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);
+    fitness_score_thresh = params.template param<double>("fitness_score_thresh", 2.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());

diff --git a/rviz/offline_correction.rviz b/rviz/offline_correction.rviz
diff --git a/src/hdl_graph_slam/graph_slam.cpp b/src/hdl_graph_slam/graph_slam.cpp
@@ -7,6 +7,7 @@
 #include <g2o/core/linear_solver.h>
 #include <g2o/core/sparse_optimizer.h>
 #include <g2o/core/robust_kernel_factory.h>
+#include <g2o/core/optimization_algorithm.h>
 #include <g2o/core/optimization_algorithm_factory.h>
 #include <g2o/solvers/pcg/linear_solver_pcg.h>
 #include <g2o/types/slam3d/types_slam3d.h>
@@ -46,7 +47,7 @@ GraphSLAM::GraphSLAM(const std::string& solver_type) {
   graph.reset(new g2o::SparseOptimizer());
   g2o::SparseOptimizer* graph = dynamic_cast<g2o::SparseOptimizer*>(this->graph.get());
 
-  std::cout << "construct solver... " << std::endl;
+  std::cout << "construct solver: " << solver_type << std::endl;
   g2o::OptimizationAlgorithmFactory* solver_factory = g2o::OptimizationAlgorithmFactory::instance();
   g2o::OptimizationAlgorithmProperty solver_property;
   g2o::OptimizationAlgorithm* solver = solver_factory->construct(solver_type, solver_property);
@@ -72,6 +73,26 @@ GraphSLAM::~GraphSLAM() {
   graph.reset();
 }
 
+void GraphSLAM::set_solver(const std::string& solver_type) {
+  g2o::SparseOptimizer* graph = dynamic_cast<g2o::SparseOptimizer*>(this->graph.get());
+
+  std::cout << "construct solver: " << solver_type << std::endl;
+  g2o::OptimizationAlgorithmFactory* solver_factory = g2o::OptimizationAlgorithmFactory::instance();
+  g2o::OptimizationAlgorithmProperty solver_property;
+  g2o::OptimizationAlgorithm* solver = solver_factory->construct(solver_type, solver_property);
+  graph->setAlgorithm(solver);
+
+  if(!graph->solver()) {
+    std::cerr << std::endl;
+    std::cerr << "error : failed to allocate solver!!" << std::endl;
+    solver_factory->listSolvers(std::cerr);
+    std::cerr << "-------------" << std::endl;
+    std::cin.ignore(1);
+    return;
+  }
+  std::cout << "done" << std::endl;
+}
+
 int GraphSLAM::num_vertices() const { return graph->vertices().size(); }
 int GraphSLAM::num_edges() const { return graph->edges().size(); }