From a534d5afb6fcf2a6ddd14f95e2281a69e6bca246 Mon Sep 17 00:00:00 2001 From: Rainer Kuemmerle Date: Sun, 26 Nov 2023 11:02:35 +0100 Subject: [PATCH] Drop custom isnan, isfinite Use std functions --- .github/workflows/windows.yml | 2 +- g2o/core/base_vertex.h | 2 -- g2o/core/base_vertex.hpp | 2 +- g2o/core/block_solver.hpp | 9 ++++++-- g2o/core/optimization_algorithm_levenberg.cpp | 7 +++--- g2o/core/sparse_optimizer.cpp | 19 ++++++++++++---- .../structure_only/structure_only_solver.h | 11 +++++----- g2o/stuff/macros.h | 13 ----------- g2o/stuff/misc.h | 22 +++++-------------- g2o/stuff/string_tools.cpp | 4 +++- g2o/types/icp/types_icp.cpp | 2 +- g2o/types/sba/edge_project_p2mc.cpp | 2 +- g2o/types/sba/edge_project_p2sc.cpp | 4 +++- unit_test/stuff/misc_tests.cpp | 12 ++++------ 14 files changed, 51 insertions(+), 60 deletions(-) diff --git a/.github/workflows/windows.yml b/.github/workflows/windows.yml index bae3e2f7d..15b84a86c 100644 --- a/.github/workflows/windows.yml +++ b/.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: | diff --git a/g2o/core/base_vertex.h b/g2o/core/base_vertex.h index 6f866c372..a655929ff 100644 --- a/g2o/core/base_vertex.h +++ b/g2o/core/base_vertex.h @@ -33,8 +33,6 @@ #include #include -#include "creators.h" -#include "g2o/stuff/macros.h" #include "optimizable_graph.h" namespace g2o { diff --git a/g2o/core/base_vertex.hpp b/g2o/core/base_vertex.hpp index 941713023..d76ff9c5e 100644 --- a/g2o/core/base_vertex.hpp +++ b/g2o/core/base_vertex.hpp @@ -37,7 +37,7 @@ double BaseVertex::solveDirect(double lambda) { G2O_VERTEX_DIM, G2O_VERTEX_DIM) * lambda; double det = tempA.determinant(); - if (g2o_isnan(det) || det < std::numeric_limits::epsilon()) + if (std::isnan(det) || det < std::numeric_limits::epsilon()) return det; Eigen::Matrix dx = tempA.llt().solve(_b); oplus(&dx[0]); diff --git a/g2o/core/block_solver.hpp b/g2o/core/block_solver.hpp index 3bbdd50f8..f67d6ea61 100644 --- a/g2o/core/block_solver.hpp +++ b/g2o/core/block_solver.hpp @@ -29,6 +29,7 @@ #include #include +#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::buildSystem() { const OptimizableGraph::Vertex* v = static_cast(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 {}", diff --git a/g2o/core/optimization_algorithm_levenberg.cpp b/g2o/core/optimization_algorithm_levenberg.cpp index 0f4f49f24..92745f8fa 100644 --- a/g2o/core/optimization_algorithm_levenberg.cpp +++ b/g2o/core/optimization_algorithm_levenberg.cpp @@ -27,6 +27,7 @@ #include "optimization_algorithm_levenberg.h" #include +#include #include #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; } diff --git a/g2o/core/sparse_optimizer.cpp b/g2o/core/sparse_optimizer.cpp index 60074001e..db6c1b174 100644 --- a/g2o/core/sparse_optimizer.cpp +++ b/g2o/core/sparse_optimizer.cpp @@ -28,24 +28,35 @@ #include #include -#include #include -#include #include #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; diff --git a/g2o/solvers/structure_only/structure_only_solver.h b/g2o/solvers/structure_only/structure_only_solver.h index 019af254b..0f5a40b3f 100644 --- a/g2o/solvers/structure_only/structure_only_solver.h +++ b/g2o/solvers/structure_only/structure_only_solver.h @@ -29,10 +29,11 @@ #define G2O_STRUCTURE_ONLY_SOLVER_H #include +#include +#include -#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(); diff --git a/g2o/stuff/macros.h b/g2o/stuff/macros.h index 0b9294eef..0e876110b 100644 --- a/g2o/stuff/macros.h +++ b/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 -#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 -#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++ diff --git a/g2o/stuff/misc.h b/g2o/stuff/misc.h index e418a5537..0a817ea71 100644 --- a/g2o/stuff/misc.h +++ b/g2o/stuff/misc.h @@ -28,10 +28,7 @@ #define G2O_STUFF_MISC_H #include -#include - -#include "g2o/config.h" -#include "macros.h" +#include /** @addtogroup utils **/ // @{ @@ -49,8 +46,11 @@ template > struct CmpPairFirst { bool operator()(const std::pair& left, const std::pair& 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. */ diff --git a/g2o/stuff/string_tools.cpp b/g2o/stuff/string_tools.cpp index 474aa3949..4eaa6fbeb 100644 --- a/g2o/stuff/string_tools.cpp +++ b/g2o/stuff/string_tools.cpp @@ -34,6 +34,8 @@ #if (defined(UNIX) || defined(CYGWIN)) && !defined(ANDROID) #include +#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 } diff --git a/g2o/types/icp/types_icp.cpp b/g2o/types/icp/types_icp.cpp index f986e193b..c84ac1632 100644 --- a/g2o/types/icp/types_icp.cpp +++ b/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; } diff --git a/g2o/types/sba/edge_project_p2mc.cpp b/g2o/types/sba/edge_project_p2mc.cpp index 230a74bbf..0c58b4e59 100644 --- a/g2o/types/sba/edge_project_p2mc.cpp +++ b/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(); } diff --git a/g2o/types/sba/edge_project_p2sc.cpp b/g2o/types/sba/edge_project_p2sc.cpp index f3133d947..e3ee39c8d 100644 --- a/g2o/types/sba/edge_project_p2sc.cpp +++ b/g2o/types/sba/edge_project_p2sc.cpp @@ -26,6 +26,8 @@ #include "edge_project_p2sc.h" +#include + 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(); } diff --git a/unit_test/stuff/misc_tests.cpp b/unit_test/stuff/misc_tests.cpp index 93e7783ca..af54d5962 100644 --- a/unit_test/stuff/misc_tests.cpp +++ b/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 + #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::quiet_NaN(); - EXPECT_EQ(std::make_pair(true, i), aux(data, size)); + EXPECT_TRUE(Eigen::VectorXd::MapType(data, size).array().isNaN().any()); } }