Drop non-needed utility functions

RainerKuemmerle · Nov 26, 2023 · dfaa267 · dfaa267
1 parent 82ef06d
commit dfaa267
Show file tree

Hide file tree

Showing 3 changed files with 2 additions and 100 deletions.
diff --git a/g2o/examples/tutorial_slam2d/simulator.cpp b/g2o/examples/tutorial_slam2d/simulator.cpp
@@ -157,7 +157,7 @@ void Simulator::simulate(int numNodes, const SE2& sensorOffset) {
             do {
               offx = Sampler::uniformRand(-0.5 * stepLen, 0.5 * stepLen);
               offy = Sampler::uniformRand(-0.5 * stepLen, 0.5 * stepLen);
-            } while (hypot_sqr(offx, offy) < 0.25 * 0.25);
+            } while (std::hypot(offx, offy) < 0.25);
             l->truePose[0] = cx + offx;
             l->truePose[1] = cy + offy;
             landmarksForCell.push_back(l);
@@ -185,9 +185,7 @@ void Simulator::simulate(int numNodes, const SE2& sensorOffset) {
         if (landmarksForCell.size() == 0) continue;
         for (size_t i = 0; i < landmarksForCell.size(); ++i) {
           Landmark* l = landmarksForCell[i];
-          double dSqr =
-              hypot_sqr(pv.truePose.translation().x() - l->truePose.x(),
-                        pv.truePose.translation().y() - l->truePose.y());
+          double dSqr = (pv.truePose.translation() - l->truePose).squaredNorm();
           if (dSqr > maxSensorSqr) continue;
           double obs = Sampler::uniformRand(0.0, 1.0);
           if (obs > observationProb)  // we do not see this one...

diff --git a/g2o/stuff/misc.h b/g2o/stuff/misc.h
@@ -48,30 +48,6 @@ inline constexpr double cst(long double v) { return (double)v; }
 
 constexpr double const_pi() { return cst(3.14159265358979323846); }
 
-/**
- * return the square value
- */
-template <typename T>
-inline T square(T x) {
-  return x * x;
-}
-
-/**
- * return the hypot of x and y
- */
-template <typename T>
-inline T hypot(T x, T y) {
-  return (T)(std::sqrt(x * x + y * y));
-}
-
-/**
- * return the squared hypot of x and y
- */
-template <typename T>
-inline T hypot_sqr(T x, T y) {
-  return x * x + y * y;
-}
-
 /**
  * convert from degree to radian
  */
@@ -93,62 +69,6 @@ inline double normalize_theta(double theta) {
   return result - const_pi();
 }
 
-/**
- * inverse of an angle, i.e., +180 degree
- */
-inline double inverse_theta(double th) {
-  return normalize_theta(th + const_pi());
-}
-
-/**
- * average two angles
- */
-inline double average_angle(double theta1, double theta2) {
-  double x, y;
-
-  x = std::cos(theta1) + std::cos(theta2);
-  y = std::sin(theta1) + std::sin(theta2);
-  if (x == 0 && y == 0)
-    return 0;
-  else
-    return std::atan2(y, x);
-}
-
-/**
- * sign function.
- * @return the sign of x. +1 for x > 0, -1 for x < 0, 0 for x == 0
- */
-template <typename T>
-inline int sign(T x) {
-  if (x > 0)
-    return 1;
-  else if (x < 0)
-    return -1;
-  else
-    return 0;
-}
-
-/**
- * clamp x to the interval [l, u]
- */
-template <typename T>
-inline T clamp(T l, T x, T u) {
-  if (x < l) return l;
-  if (x > u) return u;
-  return x;
-}
-
-/**
- * wrap x to be in the interval [l, u]
- */
-template <typename T>
-inline T wrap(T l, T x, T u) {
-  T intervalWidth = u - l;
-  while (x < l) x += intervalWidth;
-  while (x > u) x -= intervalWidth;
-  return x;
-}
-
 /**
  * The following two functions are used to force linkage with static libraries.
  */

diff --git a/unit_test/stuff/misc_tests.cpp b/unit_test/stuff/misc_tests.cpp
@@ -112,19 +112,3 @@ TEST(Stuff, ArrayHasNaN) {
     EXPECT_TRUE(Eigen::VectorXd::MapType(data, size).array().isNaN().any());
   }
 }
-
-TEST(Stuff, Square) {
-  EXPECT_DOUBLE_EQ(4, g2o::square(2));
-  EXPECT_DOUBLE_EQ(4, g2o::square(-2));
-}
-
-TEST(Stuff, Hypot) {
-  EXPECT_DOUBLE_EQ(hypot(2, 0), 2);
-  EXPECT_DOUBLE_EQ(hypot(-2, 0), 2);
-  EXPECT_DOUBLE_EQ(hypot(0, 2), 2);
-  EXPECT_DOUBLE_EQ(hypot(0, -2), 2);
-  EXPECT_DOUBLE_EQ(hypot(3, 3), sqrt(18));
-  EXPECT_DOUBLE_EQ(hypot(-3, 3), sqrt(18));
-  EXPECT_DOUBLE_EQ(hypot(-3, 3), sqrt(18));
-  EXPECT_DOUBLE_EQ(hypot(3, -3), sqrt(18));
-}