colmap · ahojnnes · Jul 8, 2023 · Jul 8, 2023 · Jul 8, 2023 · Jul 8, 2023
diff --git a/src/colmap/base/camera_rig_test.cc b/src/colmap/base/camera_rig_test.cc
@@ -197,7 +197,7 @@ BOOST_AUTO_TEST_CASE(TestComputeScale) {
   BOOST_CHECK_EQUAL(camera_rig.ComputeScale(reconstruction), 2.0);
 
   reconstruction.Image(1).SetTvec(Eigen::Vector3d(0, 0, 0));
-  BOOST_CHECK(IsNaN(camera_rig.ComputeScale(reconstruction)));
+  BOOST_CHECK(std::isnan(camera_rig.ComputeScale(reconstruction)));
 }
 
 BOOST_AUTO_TEST_CASE(TestComputeRelativePoses) {

diff --git a/src/colmap/base/image.h b/src/colmap/base/image.h
@@ -310,7 +310,7 @@ inline double Image::QvecPrior(const size_t idx) const {
 
 inline double& Image::QvecPrior(const size_t idx) { return qvec_prior_(idx); }
 
-inline bool Image::HasQvecPrior() const { return !IsNaN(qvec_prior_.sum()); }
+inline bool Image::HasQvecPrior() const { return qvec_prior_.array().isFinite().all(); }
 
 void Image::SetQvecPrior(const Eigen::Vector4d& qvec) { qvec_prior_ = qvec; }
 
@@ -334,7 +334,7 @@ inline double Image::TvecPrior(const size_t idx) const {
 
 inline double& Image::TvecPrior(const size_t idx) { return tvec_prior_(idx); }
 
-inline bool Image::HasTvecPrior() const { return !IsNaN(tvec_prior_.sum()); }
+inline bool Image::HasTvecPrior() const { return tvec_prior_.array().isFinite().all(); }
 
 void Image::SetTvecPrior(const Eigen::Vector3d& tvec) { tvec_prior_ = tvec; }
 

diff --git a/src/colmap/base/image_test.cc b/src/colmap/base/image_test.cc
@@ -53,16 +53,11 @@ BOOST_AUTO_TEST_CASE(TestDefault) {
   BOOST_CHECK_EQUAL(image.Qvec(1), 0.0);
   BOOST_CHECK_EQUAL(image.Qvec(2), 0.0);
   BOOST_CHECK_EQUAL(image.Qvec(3), 0.0);
-  BOOST_CHECK(IsNaN(image.QvecPrior(0)));
-  BOOST_CHECK(IsNaN(image.QvecPrior(1)));
-  BOOST_CHECK(IsNaN(image.QvecPrior(2)));
-  BOOST_CHECK(IsNaN(image.QvecPrior(3)));
+  BOOST_CHECK(image.QvecPrior().array().isNaN().all());
   BOOST_CHECK_EQUAL(image.Tvec(0), 0.0);
   BOOST_CHECK_EQUAL(image.Tvec(1), 0.0);
   BOOST_CHECK_EQUAL(image.Tvec(2), 0.0);
-  BOOST_CHECK(IsNaN(image.TvecPrior(0)));
-  BOOST_CHECK(IsNaN(image.TvecPrior(1)));
-  BOOST_CHECK(IsNaN(image.TvecPrior(2)));
+  BOOST_CHECK(image.TvecPrior().array().isNaN().all());
   BOOST_CHECK_EQUAL(image.HasQvecPrior(), false);
   BOOST_CHECK_EQUAL(image.HasTvecPrior(), false);
   BOOST_CHECK_EQUAL(image.Points2D().size(), 0);
@@ -222,10 +217,7 @@ BOOST_AUTO_TEST_CASE(TestQvec) {
 
 BOOST_AUTO_TEST_CASE(TestQvecPrior) {
   Image image;
-  BOOST_CHECK(IsNaN(image.QvecPrior(0)));
-  BOOST_CHECK(IsNaN(image.QvecPrior(1)));
-  BOOST_CHECK(IsNaN(image.QvecPrior(2)));
-  BOOST_CHECK(IsNaN(image.QvecPrior(3)));
+  BOOST_CHECK(image.QvecPrior().array().isNaN().all());
   BOOST_CHECK_EQUAL(image.HasQvecPrior(), false);
   image.QvecPrior(0) = 2.0;
   BOOST_CHECK_EQUAL(image.HasQvecPrior(), false);
@@ -260,9 +252,7 @@ BOOST_AUTO_TEST_CASE(TestTvec) {
 
 BOOST_AUTO_TEST_CASE(TestTvecPrior) {
   Image image;
-  BOOST_CHECK(IsNaN(image.TvecPrior(0)));
-  BOOST_CHECK(IsNaN(image.TvecPrior(1)));
-  BOOST_CHECK(IsNaN(image.TvecPrior(2)));
+  BOOST_CHECK(image.TvecPrior().array().isNaN().all());
   BOOST_CHECK_EQUAL(image.HasTvecPrior(), false);
   image.TvecPrior(0) = 2.0;
   BOOST_CHECK_EQUAL(image.HasTvecPrior(), false);

diff --git a/src/colmap/base/visibility_pyramid.cc b/src/colmap/base/visibility_pyramid.cc
@@ -102,12 +102,8 @@ void VisibilityPyramid::CellForPoint(const double x,
   CHECK_GT(width_, 0);
   CHECK_GT(height_, 0);
   const int max_dim = 1 << pyramid_.size();
-  *cx = Clip<size_t>(static_cast<size_t>(max_dim * x / width_),
-                     0,
-                     static_cast<size_t>(max_dim - 1));
-  *cy = Clip<size_t>(static_cast<size_t>(max_dim * y / height_),
-                     0,
-                     static_cast<size_t>(max_dim - 1));
+  *cx = Clamp<size_t>(max_dim * x / width_, 0, max_dim - 1);
+  *cy = Clamp<size_t>(max_dim * y / height_, 0, max_dim - 1);
 }
 
 }  // namespace colmap
diff --git a/src/colmap/estimators/pose.cc b/src/colmap/estimators/pose.cc
@@ -158,7 +158,7 @@ bool EstimateAbsolutePose(const AbsolutePoseEstimationOptions& options,
   *qvec = RotationMatrixToQuaternion(proj_matrix.leftCols<3>());
   *tvec = proj_matrix.rightCols<1>();
 
-  if (IsNaN(*qvec) || IsNaN(*tvec)) {
+  if (qvec->array().isNaN().any() || tvec->array().isNaN().any()) {
     return false;
   }
 
@@ -197,7 +197,7 @@ size_t EstimateRelativePose(const RANSACOptions& ransac_options,
 
   *qvec = RotationMatrixToQuaternion(R);
 
-  if (IsNaN(*qvec) || IsNaN(*tvec)) {
+  if (qvec->array().isNaN().any() || tvec->array().isNaN().any()) {
     return 0;
   }
 

diff --git a/src/colmap/geometry/gps.cc b/src/colmap/geometry/gps.cc
@@ -95,7 +95,7 @@ std::vector<Eigen::Vector3d> GPSTransform::XYZToEll(
 
     // Latitude
     double lat = atan2(z, radius_xy);
-    double alt;
+    double alt = 0.0;
 
     for (size_t j = 0; j < 100; ++j) {
       const double sin_lat0 = sin(lat);

diff --git a/src/colmap/geometry/pose.cc b/src/colmap/geometry/pose.cc
@@ -53,9 +53,9 @@ void RotationMatrixToEulerAngles(const Eigen::Matrix3d& R,
   *ry = std::asin(-R(2, 0));
   *rz = std::atan2(R(1, 0), R(0, 0));
 
-  *rx = IsNaN(*rx) ? 0 : *rx;
-  *ry = IsNaN(*ry) ? 0 : *ry;
-  *rz = IsNaN(*rz) ? 0 : *rz;
+  *rx = std::isnan(*rx) ? 0 : *rx;
+  *ry = std::isnan(*ry) ? 0 : *ry;
+  *rz = std::isnan(*rz) ? 0 : *rz;
 }
 
 Eigen::Matrix3d EulerAnglesToRotationMatrix(const double rx,

diff --git a/src/colmap/image/undistortion.cc b/src/colmap/image/undistortion.cc
@@ -927,8 +927,8 @@ Camera UndistortCamera(const UndistortCameraOptions& options,
                             max_scale_y * (1.0 - options.blank_pixels));
 
     // Clip the scaling factors.
-    scale_x = Clip(scale_x, options.min_scale, options.max_scale);
-    scale_y = Clip(scale_y, options.min_scale, options.max_scale);
+    scale_x = Clamp(scale_x, options.min_scale, options.max_scale);
+    scale_y = Clamp(scale_y, options.min_scale, options.max_scale);
 
     // Scale undistorted camera dimensions.
     const size_t orig_undistorted_camera_width = undistorted_camera.Width();

diff --git a/src/colmap/util/math.h b/src/colmap/util/math.h
@@ -51,17 +51,9 @@ namespace colmap {
 template <typename T>
 int SignOfNumber(T val);
 
-// Check if the given floating point number is a not-a-number (NaN) value.
-inline bool IsNaN(float x);
-inline bool IsNaN(double x);
-
-// Check if the given floating point number is a infinity.
-inline bool IsInf(float x);
-inline bool IsInf(double x);
-
-// Clip the given value to a low and maximum value.
+// Clamp the given value to a low and maximum value.
 template <typename T>
-inline T Clip(const T& value, const T& low, const T& high);
+inline T Clamp(const T& value, const T& low, const T& high);
 
 // Convert angle in degree to radians.
 inline float DegToRad(float deg);
@@ -181,14 +173,8 @@ int SignOfNumber(const T val) {
   return (T(0) < val) - (val < T(0));
 }
 
-bool IsNaN(const float x) { return x != x; }
-bool IsNaN(const double x) { return x != x; }
-
-bool IsInf(const float x) { return !IsNaN(x) && IsNaN(x - x); }
-bool IsInf(const double x) { return !IsNaN(x) && IsNaN(x - x); }
-
 template <typename T>
-T Clip(const T& value, const T& low, const T& high) {
+T Clamp(const T& value, const T& low, const T& high) {
   return std::max(low, std::min(value, high));
 }
 

diff --git a/src/colmap/util/math_test.cc b/src/colmap/util/math_test.cc
@@ -45,30 +45,14 @@ BOOST_AUTO_TEST_CASE(TestSignOfNumber) {
   BOOST_CHECK_EQUAL(SignOfNumber(-std::numeric_limits<float>::infinity()), -1);
 }
 
-BOOST_AUTO_TEST_CASE(TestIsNaN) {
-  BOOST_CHECK(!IsNaN(0.0f));
-  BOOST_CHECK(!IsNaN(0.0));
-  BOOST_CHECK(IsNaN(std::numeric_limits<float>::quiet_NaN()));
-  BOOST_CHECK(IsNaN(std::numeric_limits<double>::quiet_NaN()));
-}
-
-BOOST_AUTO_TEST_CASE(TestIsInf) {
-  BOOST_CHECK(!IsInf(0.0f));
-  BOOST_CHECK(!IsInf(0.0));
-  BOOST_CHECK(IsInf(std::numeric_limits<float>::infinity()));
-  BOOST_CHECK(IsInf(std::numeric_limits<double>::infinity()));
-  BOOST_CHECK(IsInf(-std::numeric_limits<float>::infinity()));
-  BOOST_CHECK(IsInf(-std::numeric_limits<double>::infinity()));
-}
-
-BOOST_AUTO_TEST_CASE(TestClip) {
-  BOOST_CHECK_EQUAL(Clip(0, -1, 1), 0);
-  BOOST_CHECK_EQUAL(Clip(0, 0, 1), 0);
-  BOOST_CHECK_EQUAL(Clip(0, -1, 0), 0);
-  BOOST_CHECK_EQUAL(Clip(0, -1, 1), 0);
-  BOOST_CHECK_EQUAL(Clip(0, 1, 2), 1);
-  BOOST_CHECK_EQUAL(Clip(0, -2, -1), -1);
-  BOOST_CHECK_EQUAL(Clip(0, 0, 0), 0);
+BOOST_AUTO_TEST_CASE(TestClamp) {
+  BOOST_CHECK_EQUAL(Clamp(0, -1, 1), 0);
+  BOOST_CHECK_EQUAL(Clamp(0, 0, 1), 0);
+  BOOST_CHECK_EQUAL(Clamp(0, -1, 0), 0);
+  BOOST_CHECK_EQUAL(Clamp(0, -1, 1), 0);
+  BOOST_CHECK_EQUAL(Clamp(0, 1, 2), 1);
+  BOOST_CHECK_EQUAL(Clamp(0, -2, -1), -1);
+  BOOST_CHECK_EQUAL(Clamp(0, 0, 0), 0);
 }
 
 BOOST_AUTO_TEST_CASE(TestDegToRad) {

diff --git a/src/colmap/util/matrix.h b/src/colmap/util/matrix.h
@@ -37,14 +37,6 @@
 
 namespace colmap {
 
-// Check if the given floating point array contains a NaN value.
-template <typename Derived>
-inline bool IsNaN(const Eigen::MatrixBase<Derived>& x);
-
-// Check if the given floating point array contains infinity.
-template <typename Derived>
-inline bool IsInf(const Eigen::MatrixBase<Derived>& x);
-
 // Perform RQ decomposition on matrix. The RQ decomposition transforms a matrix
 // A into the product of an upper triangular matrix R (also known as
 // right-triangular) and an orthogonal matrix Q.
@@ -55,16 +47,6 @@ void DecomposeMatrixRQ(const MatrixType& A, MatrixType* R, MatrixType* Q);
 // Implementation
 ////////////////////////////////////////////////////////////////////////////////
 
-template <typename Derived>
-bool IsNaN(const Eigen::MatrixBase<Derived>& x) {
-  return !(x.array() == x.array()).all();
-}
-
-template <typename Derived>
-bool IsInf(const Eigen::MatrixBase<Derived>& x) {
-  return !((x - x).array() == (x - x).array()).all();
-}
-
 template <typename MatrixType>
 void DecomposeMatrixRQ(const MatrixType& A, MatrixType* R, MatrixType* Q) {
   const MatrixType A_flipud_transpose =

diff --git a/src/colmap/util/matrix_test.cc b/src/colmap/util/matrix_test.cc
@@ -36,24 +36,6 @@
 
 namespace colmap {
 
-BOOST_AUTO_TEST_CASE(TestIsNaN) {
-  BOOST_CHECK(!IsNaN(Eigen::Vector3f::Zero()));
-  BOOST_CHECK(!IsNaN(Eigen::Vector3d::Zero()));
-  BOOST_CHECK(IsNaN(
-      Eigen::Vector3f(std::numeric_limits<float>::quiet_NaN(), 0.0f, 0.0f)));
-  BOOST_CHECK(IsNaN(
-      Eigen::Vector3d(std::numeric_limits<double>::quiet_NaN(), 0.0f, 0.0f)));
-}
-
-BOOST_AUTO_TEST_CASE(TestIsInf) {
-  BOOST_CHECK(!IsInf(Eigen::Vector3f::Zero()));
-  BOOST_CHECK(!IsInf(Eigen::Vector3d::Zero()));
-  BOOST_CHECK(IsInf(
-      Eigen::Vector3f(std::numeric_limits<float>::infinity(), 0.0f, 0.0f)));
-  BOOST_CHECK(IsInf(
-      Eigen::Vector3d(std::numeric_limits<double>::infinity(), 0.0f, 0.0f)));
-}
-
 BOOST_AUTO_TEST_CASE(TestDecomposeMatrixRQ) {
   for (int i = 0; i < 10; ++i) {
     const Eigen::Matrix4d A = Eigen::Matrix4d::Random();