String tools

.github/workflows/windows.yml

@@ -58,7 +58,7 @@ jobs:
           type ${{github.workspace}}/build/g2o/config.h
 
       - name: Build
-        run: cmake --build ${{github.workspace}}/build -j 2
+        run: cmake --build ${{github.workspace}}/build -j 2 --config ${{matrix.config.build_type}}
 
       - name: Test
         run: |

g2o/core/base_vertex.h

@@ -33,8 +33,6 @@
 #include <cassert>
 #include <stack>
 
-#include "creators.h"
-#include "g2o/stuff/macros.h"
 #include "optimizable_graph.h"
 
 namespace g2o {

g2o/core/base_vertex.hpp

@@ -37,7 +37,7 @@
                      G2O_VERTEX_DIM, G2O_VERTEX_DIM) *
                      lambda;
   double det = tempA.determinant();
-  if (g2o_isnan(det) || det < std::numeric_limits<double>::epsilon())
+  if (std::isnan(det) || det < std::numeric_limits<double>::epsilon())
     return det;
   Eigen::Matrix<double, D, 1, Eigen::ColMajor> dx = tempA.llt().solve(_b);
   oplus(&dx[0]);

g2o/core/block_solver.hpp

@@ -29,6 +29,7 @@
 #include <fstream>
 #include <iomanip>
 
+#include "g2o/core/eigen_types.h"
 #include "g2o/stuff/logger.h"
 #include "g2o/stuff/macros.h"
 #include "g2o/stuff/misc.h"
@@ -530,8 +531,12 @@ bool BlockSolver<Traits>::buildSystem() {
       const OptimizableGraph::Vertex* v =
           static_cast<const OptimizableGraph::Vertex*>(e->vertex(i));
       if (!v->fixed()) {
-        bool hasANan = arrayHasNaN(jacobianWorkspace.workspaceForVertex(i),
-                                   e->dimension() * v->dimension());
+        bool hasANan =
+            g2o::VectorX::MapType(jacobianWorkspace.workspaceForVertex(i),
+                                  e->dimension() * v->dimension())
+                .array()
+                .isNaN()
+                .any();
         if (hasANan) {
           G2O_WARN(
               "buildSystem(): NaN within Jacobian for edge {} for vertex {}",

g2o/core/optimization_algorithm_levenberg.cpp

@@ -27,6 +27,7 @@
 #include "optimization_algorithm_levenberg.h"
 
 #include <cassert>
+#include <cmath>
 #include <iostream>
 
 #include "batch_stats.h"
@@ -126,7 +127,7 @@ OptimizationAlgorithm::SolverResult OptimizationAlgorithmLevenberg::solve(
         ok2 ? computeScale() + cst(1e-3) : 1;  // make sure it's non-zero :)
     rho /= scale;
 
-    if (rho > 0 && g2o_isfinite(tempChi) && ok2) {  // last step was good
+    if (rho > 0 && std::isfinite(tempChi) && ok2) {  // last step was good
       double alpha = 1. - pow((2 * rho - 1), 3);
       // crop lambda between minimum and maximum factors
       alpha = (std::min)(alpha, _goodStepUpperScale);
@@ -139,14 +140,14 @@ OptimizationAlgorithm::SolverResult OptimizationAlgorithmLevenberg::solve(
       _currentLambda *= _ni;
       _ni *= 2;
       _optimizer->pop();  // restore the last state before trying to optimize
-      if (!g2o_isfinite(_currentLambda)) break;
+      if (!std::isfinite(_currentLambda)) break;
     }
     qmax++;
   } while (rho < 0 && qmax < _maxTrialsAfterFailure->value() &&
            !_optimizer->terminate());
 
   if (qmax == _maxTrialsAfterFailure->value() || rho == 0 ||
-      !g2o_isfinite(_currentLambda))
+      !std::isfinite(_currentLambda))
     return Terminate;
   return OK;
 }

g2o/core/sparse_block_matrix.h

@@ -37,7 +37,6 @@
 #include <vector>
 
 #include "g2o/config.h"
-#include "g2o/stuff/misc.h"
 #include "g2o/stuff/sparse_helper.h"
 #include "matrix_operations.h"
 #include "matrix_structure.h"

g2o/core/sparse_block_matrix.hpp

@@ -700,8 +700,9 @@
     for (typename HashSparseColumn::const_iterator it = column.begin();
          it != column.end(); ++it)
       sparseRowSorted.push_back(*it);
-    std::sort(sparseRowSorted.begin(), sparseRowSorted.end(),
-              CmpPairFirst<int, MatrixType*>());
+    std::sort(
+        sparseRowSorted.begin(), sparseRowSorted.end(),
+        [](const auto& lhs, const auto& rhs) { return lhs.first < rhs.first; });
     // try to free some memory early
     HashSparseColumn aux;
     std::swap(aux, column);

g2o/core/sparse_optimizer.cpp

@@ -28,24 +28,35 @@
 
 #include <algorithm>
 #include <cassert>
-#include <iomanip>
 #include <iostream>
-#include <iterator>
 #include <utility>
 
 #include "batch_stats.h"
 #include "estimate_propagator.h"
-#include "g2o/config.h"
+#include "g2o/config.h"  // IWYU pragma: keep
 #include "g2o/core/optimizable_graph.h"
 #include "g2o/core/ownership.h"
 #include "g2o/stuff/logger.h"
 #include "g2o/stuff/macros.h"
-#include "g2o/stuff/misc.h"
 #include "g2o/stuff/timeutil.h"
 #include "hyper_graph_action.h"
 #include "optimization_algorithm.h"
 #include "robust_kernel.h"
 
+namespace {
+/**
+ * tests whether there is a NaN in the array
+ */
+bool arrayHasNaN(const double* array, int size, int* nanIndex = 0) {
+  for (int i = 0; i < size; ++i)
+    if (std::isnan(array[i])) {
+      if (nanIndex) *nanIndex = i;
+      return true;
+    }
+  return false;
+}
+}  // namespace
+
 namespace g2o {
 using namespace std;
 

g2o/core/sparse_optimizer_terminate_action.cpp

@@ -35,7 +35,7 @@ namespace g2o {
 
 SparseOptimizerTerminateAction::SparseOptimizerTerminateAction()
     : HyperGraphAction(),
-      _gainThreshold(cst(1e-6)),
+      _gainThreshold(1e-6),
       _lastChi(0),
       _auxTerminateFlag(false),
       _maxIterations(std::numeric_limits<int>::max()) {}