From 638bb0305968e891204d75254c0ca72784529fdd Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 10 Dec 2024 13:41:19 -0500 Subject: [PATCH] Update some header includes --- gtsam/basis/Chebyshev2.cpp | 8 ++++++++ gtsam/basis/Chebyshev2.h | 10 +--------- gtsam/geometry/Pose2.h | 11 +++++++---- gtsam/geometry/Rot3.h | 7 +++++-- gtsam/geometry/triangulation.h | 7 +++++-- gtsam/inference/MetisIndex.h | 7 +++++-- gtsam/linear/HessianFactor.h | 18 ++++++++++++------ gtsam/nonlinear/ExpressionFactor.h | 4 ++-- gtsam/slam/EssentialMatrixFactor.h | 5 +++-- gtsam/slam/SmartFactorBase.h | 7 +++++-- 10 files changed, 53 insertions(+), 31 deletions(-) diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp index 63fca64cc4..71c3db7f06 100644 --- a/gtsam/basis/Chebyshev2.cpp +++ b/gtsam/basis/Chebyshev2.cpp @@ -20,6 +20,14 @@ namespace gtsam { +double Chebyshev2::Point(size_t N, int j, double a, double b) { + assert(j >= 0 && size_t(j) < N); + const double dtheta = M_PI / (N > 1 ? (N - 1) : 1); + // We add -PI so that we get values ordered from -1 to +1 + // sin(-M_PI_2 + dtheta*j); also works + return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2; +} + Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) { // Allocate space for weights Weights weights(N); diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index 1374faeef7..207f4b6176 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -36,8 +36,6 @@ #include #include -#include - namespace gtsam { /** @@ -63,13 +61,7 @@ class GTSAM_EXPORT Chebyshev2 : public Basis { * @param b Upper bound of interval (default: 1) * @return double */ - static double Point(size_t N, int j, double a = -1, double b = 1) { - assert(j >= 0 && size_t(j) < N); - const double dtheta = M_PI / (N > 1 ? (N - 1) : 1); - // We add -PI so that we get values ordered from -1 to +1 - // sin(-M_PI_2 + dtheta*j); also works - return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2; - } + static double Point(size_t N, int j, double a = -1, double b = 1); /// All Chebyshev points static Vector Points(size_t N) { diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index d9cad37f36..ff9a6399e3 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -28,7 +28,6 @@ #include #include -#include namespace gtsam { @@ -82,9 +81,13 @@ class GTSAM_EXPORT Pose2: public LieGroup { Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {} /** Constructor from 3*3 matrix */ - Pose2(const Matrix &T) : - r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) { - assert(T.rows() == 3 && T.cols() == 3); + Pose2(const Matrix &T) + : r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) { +#ifndef NDEBUG + if (T.rows() != 3 || T.cols() != 3) { + throw; + } +#endif } /// @} diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 398e8d4733..d1e3304382 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -30,7 +30,6 @@ #include // Get GTSAM_USE_QUATERNIONS macro #include -#include // You can override the default coordinate mode using this flag #ifndef ROT3_DEFAULT_COORDINATES_MODE @@ -160,7 +159,11 @@ class GTSAM_EXPORT Rot3 : public LieGroup { /// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. inline static Rot3 RzRyRx(const Vector& xyz, OptionalJacobian<3, 3> H = {}) { - assert(xyz.size() == 3); +#ifndef NDEBUG + if (xyz.size() != 3) { + throw; + } +#endif Rot3 out; if (H) { Vector3 Hx, Hy, Hz; diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 0fa9d074e0..9087f01c50 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -35,7 +35,6 @@ #include #include -#include namespace gtsam { @@ -318,7 +317,11 @@ typename CAMERA::MeasurementVector undistortMeasurements( const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements) { const size_t nrMeasurements = measurements.size(); - assert(nrMeasurements == cameras.size()); +#ifndef NDEBUG + if (nrMeasurements != cameras.size()) { + throw; + } +#endif typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements); for (size_t ii = 0; ii < nrMeasurements; ++ii) { // Calibrate with cal and uncalibrate with pinhole version of cal so that diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h index cc058dfbd4..e051b9ae90 100644 --- a/gtsam/inference/MetisIndex.h +++ b/gtsam/inference/MetisIndex.h @@ -25,7 +25,6 @@ #include #include #include -#include namespace gtsam { /** @@ -93,7 +92,11 @@ class GTSAM_EXPORT MetisIndex { return nKeys_; } Key intToKey(int32_t value) const { - assert(value >= 0); +#ifndef NDEBUG + if (value < 0) { + throw; + } +#endif return intKeyBMap_.right.find(value)->second; } diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 53b6d0e8b3..56b3d6537a 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -23,8 +23,6 @@ #include #include -#include - namespace gtsam { // Forward declarations @@ -242,14 +240,18 @@ namespace gtsam { * use, for example, begin() + 2 to get the 3rd variable in this factor. * @return The linear term \f$ g \f$ */ SymmetricBlockMatrix::constBlock linearTerm(const_iterator j) const { - assert(!empty()); +#ifndef NDEBUG + if(empty()) throw; +#endif return info_.aboveDiagonalBlock(j - begin(), size()); } /** Return the complete linear term \f$ g \f$ as described above. * @return The linear term \f$ g \f$ */ SymmetricBlockMatrix::constBlock linearTerm() const { - assert(!empty()); +#ifndef NDEBUG + if(empty()) throw; +#endif // get the last column (except the bottom right block) return info_.aboveDiagonalRange(0, size(), size(), size() + 1); } @@ -257,7 +259,9 @@ namespace gtsam { /** Return the complete linear term \f$ g \f$ as described above. * @return The linear term \f$ g \f$ */ SymmetricBlockMatrix::Block linearTerm() { - assert(!empty()); +#ifndef NDEBUG + if(empty()) throw; +#endif return info_.aboveDiagonalRange(0, size(), size(), size() + 1); } @@ -326,7 +330,9 @@ namespace gtsam { * @param other the HessianFactor to be updated */ void updateHessian(HessianFactor* other) const { - assert(other); +#ifndef NDEBUG + if(!other) throw; +#endif updateHessian(other->keys_, &other->info_); } diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index b3e34d079e..63b2469a94 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -43,7 +43,7 @@ namespace gtsam { * */ template -class ExpressionFactor : public NoiseModelFactor { +class GTSAM_EXPORT ExpressionFactor : public NoiseModelFactor { GTSAM_CONCEPT_ASSERT(IsTestable); protected: @@ -246,7 +246,7 @@ struct traits > : public Testable > {}; * */ template -class ExpressionFactorN : public ExpressionFactor { +class GTSAM_EXPORT ExpressionFactorN : public ExpressionFactor { public: static const std::size_t NARY_EXPRESSION_SIZE = sizeof...(Args); using ArrayNKeys = std::array; diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 3c2f105d46..601f8e94ed 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -25,7 +25,6 @@ #include #include -#include namespace gtsam { @@ -71,7 +70,9 @@ class EssentialMatrixFactor : public NoiseModelFactorN { const SharedNoiseModel& model, std::shared_ptr K) : Base(model, key) { - assert(K); +#ifndef NDEBUG + if (K->empty()) throw; +#endif vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA)); vB_ = EssentialMatrix::Homogeneous(K->calibrate(pB)); } diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index b8e789aaa7..d3e879c33d 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -34,7 +34,6 @@ #include #endif #include -#include namespace gtsam { @@ -136,7 +135,11 @@ class SmartFactorBase: public NonlinearFactor { /// Add a bunch of measurements, together with the camera keys. void add(const ZVector& measurements, const KeyVector& cameraKeys) { - assert(measurements.size() == cameraKeys.size()); +#ifndef NDEBUG + if (measurements.size() != cameraKeys.size()) { + throw std::runtime_error("Number of measurements and camera keys do not match"); + } +#endif for (size_t i = 0; i < measurements.size(); i++) { this->add(measurements[i], cameraKeys[i]); }