Drop custom isnan, isfinite

Use std functions

.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 @@ double BaseVertex<D, T>::solveDirect(double lambda) {
                      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_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/solvers/structure_only/structure_only_solver.h

@@ -29,10 +29,11 @@
 #define G2O_STRUCTURE_ONLY_SOLVER_H
 
 #include <cassert>
+#include <cmath>
+#include <vector>
 
-#include "g2o/core/base_binary_edge.h"
-#include "g2o/core/base_vertex.h"
 #include "g2o/core/optimization_algorithm.h"
+#include "g2o/core/robust_kernel.h"
 #include "g2o/core/sparse_optimizer.h"
 
 namespace g2o {
@@ -182,9 +183,9 @@ class StructureOnlySolver : public OptimizationAlgorithm {
                   new_chi2 += e->chi2();
                 }
               }
-              assert(g2o_isnan(new_chi2) == false && "Chi is NaN");
-              double rho = (chi2 - new_chi2);
-              if (rho > 0 && g2o_isfinite(new_chi2)) {
+              assert(std::isnan(new_chi2) == false && "Chi is NaN");
+              const double rho = (chi2 - new_chi2);
+              if (rho > 0 && std::isfinite(new_chi2)) {
                 goodStep = true;
                 chi2 = new_chi2;
                 v->discardTop();

g2o/stuff/macros.h

@@ -45,10 +45,6 @@
 #define G2O_ATTRIBUTE_WARNING(func) func __attribute__((warning))
 #define G2O_ATTRIBUTE_DEPRECATED(func) func __attribute__((deprecated))
 
-#define g2o_isnan(x) std::isnan(x)
-#define g2o_isinf(x) std::isinf(x)
-#define g2o_isfinite(x) std::isfinite(x)
-
 // MSVC on Windows
 #elif defined _MSC_VER
 #define __PRETTY_FUNCTION__ __FUNCTION__
@@ -76,10 +72,6 @@ for static C++ objects. For GCC, uses a constructor attribute."
 
 #include <float.h>
 
-#define g2o_isnan(x) _isnan(x)
-#define g2o_isinf(x) (_finite(x) == 0)
-#define g2o_isfinite(x) (_finite(x) != 0)
-
 // unknown compiler
 #else
 #ifndef __PRETTY_FUNCTION__
@@ -90,11 +82,6 @@ for static C++ objects. For GCC, uses a constructor attribute."
 #define G2O_ATTRIBUTE_WARNING(func) func
 #define G2O_ATTRIBUTE_DEPRECATED(func) func
 
-#include <math.h>
-#define g2o_isnan(x) isnan(x)
-#define g2o_isinf(x) isinf(x)
-#define g2o_isfinite(x) isfinite(x)
-
 #endif
 
 // some macros that are only useful for c++

g2o/stuff/misc.h

@@ -28,10 +28,7 @@
 #define G2O_STUFF_MISC_H
 
 #include <cmath>
-#include <memory>
-
-#include "g2o/config.h"
-#include "macros.h"
+#include <functional>
 
 /** @addtogroup utils **/
 // @{
@@ -49,8 +46,11 @@ template <class T1, class T2, class Pred = std::less<T1> >
 struct CmpPairFirst {
   bool operator()(const std::pair<T1, T2>& left,
                   const std::pair<T1, T2>& right) {
-    return Pred()(left.first, right.first);
+    return pred_(left.first, right.first);
   }
+
+ private:
+  Pred pred_;
 };
 
 /**
@@ -162,18 +162,6 @@ inline T wrap(T l, T x, T u) {
   return x;
 }
 
-/**
- * tests whether there is a NaN in the array
- */
-inline bool arrayHasNaN(const double* array, int size, int* nanIndex = 0) {
-  for (int i = 0; i < size; ++i)
-    if (g2o_isnan(array[i])) {
-      if (nanIndex) *nanIndex = i;
-      return true;
-    }
-  return false;
-}
-
 /**
  * The following two functions are used to force linkage with static libraries.
  */

g2o/stuff/string_tools.cpp

@@ -34,6 +34,8 @@
 
 #if (defined(UNIX) || defined(CYGWIN)) && !defined(ANDROID)
 #include <wordexp.h>
+#else
+#include "g2o/stuff/logger.h"
 #endif
 
 namespace g2o {
@@ -93,7 +95,7 @@ std::string strExpandFilename(const std::string& filename) {
   return result;
 #else
   (void)filename;
-  G2O_WARN("{} not implemented", __PRETTY_FUNCTION__);
+  G2O_WARN("not implemented");
   return std::string();
 #endif
 }

g2o/types/icp/types_icp.cpp

@@ -214,7 +214,7 @@ void Edge_XYZ_VSC::linearizeOplus() {
   double py = pc(1);
   double pz = pc(2);
   double ipz2 = 1.0 / (pz * pz);
-  if (isnan(ipz2)) {
+  if (std::isnan(ipz2)) {
     std::cout << "[SetJac] infinite jac" << std::endl;
     *(int*)0x0 = 0;
   }

g2o/types/sba/edge_project_p2mc.cpp

@@ -84,7 +84,7 @@ void EdgeProjectP2MC::linearizeOplus() {
   double py = pc(1);
   double pz = pc(2);
   double ipz2 = 1.0 / (pz * pz);
-  if (g2o_isnan(ipz2)) {
+  if (std::isnan(ipz2)) {
     std::cout << "[SetJac] infinite jac" << std::endl;
     abort();
   }

g2o/types/sba/edge_project_p2sc.cpp

@@ -26,6 +26,8 @@
 
 #include "edge_project_p2sc.h"
 
+#include <cmath>
+
 namespace g2o {
 
 // point to camera projection, stereo
@@ -104,7 +106,7 @@ void EdgeProjectP2SC::linearizeOplus() {
   double py = pc(1);
   double pz = pc(2);
   double ipz2 = 1.0 / (pz * pz);
-  if (g2o_isnan(ipz2)) {
+  if (std::isnan(ipz2)) {
     std::cout << "[SetJac] infinite jac" << std::endl;
     abort();
   }

unit_test/stuff/misc_tests.cpp

@@ -24,6 +24,8 @@
 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
+#include <Eigen/Core>
+
 #include "g2o/stuff/misc.h"
 #include "gtest/gtest.h"
 
@@ -102,18 +104,12 @@ TEST(Stuff, ArrayHasNaN) {
   double data[size];
   std::fill_n(data, size, 0);
 
-  auto aux = [](const double* data, int size) {
-    int nanIndex = -1;
-    bool hasNan = g2o::arrayHasNaN(data, size, &nanIndex);
-    return std::make_pair(hasNan, nanIndex);
-  };
-
-  EXPECT_EQ(std::make_pair(false, -1), aux(data, size));
+  EXPECT_FALSE(Eigen::VectorXd::MapType(data, size).array().isNaN().any());
 
   for (int i = 0; i < size; ++i) {
     std::fill_n(data, size, 0);
     data[i] = std::numeric_limits<double>::quiet_NaN();
-    EXPECT_EQ(std::make_pair(true, i), aux(data, size));
+    EXPECT_TRUE(Eigen::VectorXd::MapType(data, size).array().isNaN().any());
   }
 }