Update some header includes

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);

gtsam/basis/Chebyshev2.h

@@ -36,8 +36,6 @@
 #include <gtsam/base/OptionalJacobian.h>
 #include <gtsam/basis/Basis.h>
 
-#include <cassert>
-
 namespace gtsam {
 
 /**
@@ -63,13 +61,7 @@ class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
    * @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) {

gtsam/geometry/Pose2.h

@@ -28,7 +28,6 @@
 #include <gtsam/base/std_optional_serialization.h>
 
 #include <optional>
-#include <cassert>
 
 namespace gtsam {
 
@@ -82,9 +81,13 @@ class GTSAM_EXPORT Pose2: public LieGroup<Pose2, 3> {
   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
   }
 
   /// @}

gtsam/geometry/Rot3.h

@@ -30,7 +30,6 @@
 #include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro
 
 #include <random>
-#include <cassert>
 
 // 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<Rot3, 3> {
     /// 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;

gtsam/geometry/triangulation.h

@@ -35,7 +35,6 @@
 #include <gtsam/slam/TriangulationFactor.h>
 
 #include <optional>
-#include <cassert>
 
 namespace gtsam {
 
@@ -318,7 +317,11 @@ typename CAMERA::MeasurementVector undistortMeasurements(
     const CameraSet<CAMERA>& 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

gtsam/inference/MetisIndex.h

@@ -25,7 +25,6 @@
 #include <vector>
 #include <map>
 #include <unordered_map>
-#include <cassert>
 
 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;
   }
 

gtsam/linear/HessianFactor.h

@@ -23,8 +23,6 @@
 #include <gtsam/base/SymmetricBlockMatrix.h>
 #include <gtsam/base/FastVector.h>
 
-#include <cassert>
-
 namespace gtsam {
 
   // Forward declarations
@@ -242,22 +240,28 @@ 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);
     }
 
     /** 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_);
     }
 

gtsam/nonlinear/ExpressionFactor.h

@@ -43,7 +43,7 @@ namespace gtsam {
  *
  */
 template <typename T>
-class ExpressionFactor : public NoiseModelFactor {
+class GTSAM_EXPORT ExpressionFactor : public NoiseModelFactor {
   GTSAM_CONCEPT_ASSERT(IsTestable<T>);
 
 protected:
@@ -246,7 +246,7 @@ struct traits<ExpressionFactor<T> > : public Testable<ExpressionFactor<T> > {};
  *
  */
 template <typename T, typename... Args>
-class ExpressionFactorN : public ExpressionFactor<T> {
+class GTSAM_EXPORT ExpressionFactorN : public ExpressionFactor<T> {
 public:
   static const std::size_t NARY_EXPRESSION_SIZE = sizeof...(Args);
   using ArrayNKeys = std::array<Key, NARY_EXPRESSION_SIZE>;

gtsam/slam/EssentialMatrixFactor.h

@@ -25,7 +25,6 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
 #include <iostream>
-#include <cassert>
 
 namespace gtsam {
 
@@ -71,7 +70,9 @@ class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
                         const SharedNoiseModel& model,
                         std::shared_ptr<CALIBRATION> 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));
   }

gtsam/slam/SmartFactorBase.h

@@ -34,7 +34,6 @@
 #include <boost/serialization/optional.hpp>
 #endif
 #include <vector>
-#include <cassert>
 
 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]);
     }