diff --git a/examples/CombinedImuFactorsExample.cpp b/examples/CombinedImuFactorsExample.cpp
index 6ce4f76faa..436b6e283a 100644
--- a/examples/CombinedImuFactorsExample.cpp
+++ b/examples/CombinedImuFactorsExample.cpp
@@ -48,6 +48,7 @@
 #include <cstring>
 #include <fstream>
 #include <iostream>
+#include <cassert>
 
 using namespace gtsam;
 using namespace std;
diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp
index 342f1f2203..7cbc8989fd 100644
--- a/examples/ImuFactorsExample.cpp
+++ b/examples/ImuFactorsExample.cpp
@@ -49,6 +49,7 @@
 #include <gtsam/slam/dataset.h>
 
 #include <cstring>
+#include <cassert>
 #include <fstream>
 #include <iostream>
 
diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp
index a2ad8cf0b0..bc4e6e3408 100644
--- a/examples/elaboratePoint2KalmanFilter.cpp
+++ b/examples/elaboratePoint2KalmanFilter.cpp
@@ -29,6 +29,8 @@
 #include <gtsam/linear/NoiseModel.h>
 #include <gtsam/geometry/Point2.h>
 
+#include <cassert>
+
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp
index 247c53ccea..1e04bc58a8 100644
--- a/gtsam/base/Matrix.cpp
+++ b/gtsam/base/Matrix.cpp
@@ -24,6 +24,7 @@
 #include <Eigen/LU>
 
 #include <cstdarg>
+#include <cassert>
 #include <cstring>
 #include <iomanip>
 #include <list>
diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp
index ce7aeba7b9..dbd48dfc87 100644
--- a/gtsam/base/Vector.cpp
+++ b/gtsam/base/Vector.cpp
@@ -24,6 +24,7 @@
 #include <iostream>
 #include <fstream>
 #include <sstream>
+#include <cassert>
 #include <iomanip>
 #include <cmath>
 #include <cstdio>
diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h
index 7774413005..c8fef76e9a 100644
--- a/gtsam/base/VerticalBlockMatrix.h
+++ b/gtsam/base/VerticalBlockMatrix.h
@@ -21,6 +21,8 @@
 #include <gtsam/base/MatrixSerialization.h>
 #include <gtsam/base/FastVector.h>
 
+#include <cassert>
+
 namespace gtsam {
 
   // Forward declarations
diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp
index 71cef2524e..9fee627fe5 100644
--- a/gtsam/base/cholesky.cpp
+++ b/gtsam/base/cholesky.cpp
@@ -21,6 +21,7 @@
 #include <gtsam/base/timing.h>
 
 #include <cmath>
+#include <cassert>
 
 using namespace std;
 
diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp
index e8838a4760..b14f699a31 100644
--- a/gtsam/base/tests/testNumericalDerivative.cpp
+++ b/gtsam/base/tests/testNumericalDerivative.cpp
@@ -18,6 +18,8 @@
 #include <gtsam/base/numericalDerivative.h>
 #include <CppUnitLite/TestHarness.h>
 
+#include <cassert>
+
 using namespace std;
 using namespace gtsam;
 
diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h
index 849a511049..1374faeef7 100644
--- a/gtsam/basis/Chebyshev2.h
+++ b/gtsam/basis/Chebyshev2.h
@@ -36,6 +36,8 @@
 #include <gtsam/base/OptionalJacobian.h>
 #include <gtsam/basis/Basis.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 /**
diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp
index c44905d79d..eeb5dca3f2 100644
--- a/gtsam/discrete/DiscreteConditional.cpp
+++ b/gtsam/discrete/DiscreteConditional.cpp
@@ -30,6 +30,7 @@
 #include <string>
 #include <utility>
 #include <vector>
+#include <cassert>
 
 using namespace std;
 using std::pair;
diff --git a/gtsam/discrete/DiscreteLookupDAG.cpp b/gtsam/discrete/DiscreteLookupDAG.cpp
index 4d02e007b2..d1108e7b70 100644
--- a/gtsam/discrete/DiscreteLookupDAG.cpp
+++ b/gtsam/discrete/DiscreteLookupDAG.cpp
@@ -23,6 +23,7 @@
 #include <iterator>
 #include <string>
 #include <utility>
+#include <cassert>
 
 using std::pair;
 using std::vector;
diff --git a/gtsam/geometry/Cal3f.cpp b/gtsam/geometry/Cal3f.cpp
index 319155dc91..b8e365eeda 100644
--- a/gtsam/geometry/Cal3f.cpp
+++ b/gtsam/geometry/Cal3f.cpp
@@ -28,6 +28,7 @@
 #include <gtsam/geometry/Cal3f.h>
 
 #include <iostream>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h
index 26d4952c8b..e3c16f0666 100644
--- a/gtsam/geometry/CameraSet.h
+++ b/gtsam/geometry/CameraSet.h
@@ -26,6 +26,7 @@
 #include <gtsam/inference/Key.h>
 
 #include <vector>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp
index 05678dc272..640481406e 100644
--- a/gtsam/geometry/Pose2.cpp
+++ b/gtsam/geometry/Pose2.cpp
@@ -20,6 +20,7 @@
 #include <gtsam/base/concepts.h>
 
 #include <cmath>
+#include <cassert>
 #include <iostream>
 #include <iomanip>
 
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index aad81493fb..d9cad37f36 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -28,6 +28,7 @@
 #include <gtsam/base/std_optional_serialization.h>
 
 #include <optional>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp
index a936bd02a9..10ba5894e5 100644
--- a/gtsam/geometry/Rot3.cpp
+++ b/gtsam/geometry/Rot3.cpp
@@ -23,6 +23,7 @@
 #include <gtsam/geometry/SO3.h>
 
 #include <cmath>
+#include <cassert>
 #include <random>
 
 using namespace std;
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index 7e05ee4daf..398e8d4733 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -30,6 +30,7 @@
 #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
diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h
index 7e1762b3fe..75ff872a8e 100644
--- a/gtsam/geometry/SOn.h
+++ b/gtsam/geometry/SOn.h
@@ -33,6 +33,7 @@
 #include <type_traits>
 #include <vector>
 #include <random>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h
index d82da3459a..0fa9d074e0 100644
--- a/gtsam/geometry/triangulation.h
+++ b/gtsam/geometry/triangulation.h
@@ -20,7 +20,7 @@
 
 #pragma once
 
-#include "gtsam/geometry/Point3.h"
+#include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/Cal3Fisheye.h>
 #include <gtsam/geometry/Cal3Unified.h>
@@ -35,6 +35,7 @@
 #include <gtsam/slam/TriangulationFactor.h>
 
 #include <optional>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp
index 22d3c7dd25..b9bdcbed24 100644
--- a/gtsam/hybrid/HybridJunctionTree.cpp
+++ b/gtsam/hybrid/HybridJunctionTree.cpp
@@ -22,6 +22,7 @@
 #include <gtsam/inference/Key.h>
 
 #include <unordered_map>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h
index 95d475a026..fdfa94d2b6 100644
--- a/gtsam/inference/BayesTree-inst.h
+++ b/gtsam/inference/BayesTree-inst.h
@@ -27,7 +27,7 @@
 
 #include <fstream>
 #include <queue>
-
+#include <cassert>
 namespace gtsam {
 
   /* ************************************************************************* */
diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h
index 3186246087..2a5f0b4558 100644
--- a/gtsam/inference/ClusterTree-inst.h
+++ b/gtsam/inference/ClusterTree-inst.h
@@ -19,6 +19,7 @@
 #include <mutex>
 #endif
 #include <queue>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/inference/EliminationTree-inst.h b/gtsam/inference/EliminationTree-inst.h
index e9acc12e1c..16449d0f4b 100644
--- a/gtsam/inference/EliminationTree-inst.h
+++ b/gtsam/inference/EliminationTree-inst.h
@@ -19,6 +19,7 @@
 
 #include <stack>
 #include <queue>
+#include <cassert>
 
 #include <gtsam/base/timing.h>
 #include <gtsam/base/treeTraversal-inst.h>
diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h
index 0f79c2a9a2..6a8d6918ab 100644
--- a/gtsam/inference/JunctionTree-inst.h
+++ b/gtsam/inference/JunctionTree-inst.h
@@ -25,6 +25,8 @@
 #include <gtsam/symbolic/SymbolicConditional.h>
 #include <gtsam/symbolic/SymbolicFactor-inst.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 template<class BAYESTREE, class GRAPH, class ETREE_NODE>
diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h
index abdf11c5fc..cc058dfbd4 100644
--- a/gtsam/inference/MetisIndex.h
+++ b/gtsam/inference/MetisIndex.h
@@ -25,6 +25,7 @@
 #include <vector>
 #include <map>
 #include <unordered_map>
+#include <cassert>
 
 namespace gtsam {
 /**
diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp
index cb2ca752d4..95eb17a1c5 100644
--- a/gtsam/inference/Ordering.cpp
+++ b/gtsam/inference/Ordering.cpp
@@ -18,6 +18,7 @@
 
 #include <vector>
 #include <limits>
+#include <cassert>
 
 #include <gtsam/inference/Ordering.h>
 #include <gtsam/3rdparty/CCOLAMD/Include/ccolamd.h>
diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp
index 3c3050f901..4e8ef804c0 100644
--- a/gtsam/linear/HessianFactor.cpp
+++ b/gtsam/linear/HessianFactor.cpp
@@ -25,13 +25,14 @@
 #include <gtsam/base/cholesky.h>
 #include <gtsam/base/debug.h>
 #include <gtsam/base/FastMap.h>
+#include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/ThreadsafeException.h>
 #include <gtsam/base/timing.h>
 
 #include <sstream>
+#include <cassert>
 #include <limits>
-#include "gtsam/base/Vector.h"
 
 using namespace std;
 
diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h
index 8cbabcd5da..53b6d0e8b3 100644
--- a/gtsam/linear/HessianFactor.h
+++ b/gtsam/linear/HessianFactor.h
@@ -23,6 +23,7 @@
 #include <gtsam/base/SymmetricBlockMatrix.h>
 #include <gtsam/base/FastVector.h>
 
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp
index 3cfb5ce7b8..51d513e330 100644
--- a/gtsam/linear/JacobianFactor.cpp
+++ b/gtsam/linear/JacobianFactor.cpp
@@ -32,6 +32,7 @@
 #include <gtsam/base/cholesky.h>
 
 #include <cmath>
+#include <cassert>
 #include <sstream>
 #include <stdexcept>
 
diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp
index ded6bc5e36..6291c90b6b 100644
--- a/gtsam/linear/KalmanFilter.cpp
+++ b/gtsam/linear/KalmanFilter.cpp
@@ -26,6 +26,9 @@
 #include <gtsam/linear/HessianFactor.h>
 #include <gtsam/base/Testable.h>
 
+#ifndef NDEBUG
+#include <cassert>
+#endif
 
 using namespace std;
 
diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp
index 884c87270d..39666b46bc 100644
--- a/gtsam/linear/NoiseModel.cpp
+++ b/gtsam/linear/NoiseModel.cpp
@@ -20,6 +20,7 @@
 #include <gtsam/base/timing.h>
 
 #include <cmath>
+#include <cassert>
 #include <iostream>
 #include <limits>
 #include <stdexcept>
diff --git a/gtsam/linear/Sampler.cpp b/gtsam/linear/Sampler.cpp
index 20d4c955b7..68f21d7234 100644
--- a/gtsam/linear/Sampler.cpp
+++ b/gtsam/linear/Sampler.cpp
@@ -17,6 +17,9 @@
  */
 
 #include <gtsam/linear/Sampler.h>
+
+#include <cassert>
+
 namespace gtsam {
 
 /* ************************************************************************* */
diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp
index 96f4847b56..53ea94d6eb 100644
--- a/gtsam/linear/SubgraphPreconditioner.cpp
+++ b/gtsam/linear/SubgraphPreconditioner.cpp
@@ -25,6 +25,7 @@
 #include <gtsam/base/Vector.h>
 
 #include <stdexcept>
+#include <cassert>
 
 using std::cout;
 using std::endl;
diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp
index 88e45d3187..5697d54d42 100644
--- a/gtsam/navigation/ImuFactor.cpp
+++ b/gtsam/navigation/ImuFactor.cpp
@@ -23,6 +23,7 @@
 
 /* External or standard includes */
 #include <ostream>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp
index 7f998987bd..dd86b829c5 100644
--- a/gtsam/navigation/PreintegrationBase.cpp
+++ b/gtsam/navigation/PreintegrationBase.cpp
@@ -20,9 +20,11 @@
  *  @author Varun Agrawal
  **/
 
-#include "PreintegrationBase.h"
+#include <gtsam/navigation/PreintegrationBase.h>
 #include <gtsam/base/numericalDerivative.h>
 
+#include <cassert>
+
 using namespace std;
 
 namespace gtsam {
diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.cpp b/gtsam/nonlinear/DoglegOptimizerImpl.cpp
index 4f1c6fb54e..e82a513caf 100644
--- a/gtsam/nonlinear/DoglegOptimizerImpl.cpp
+++ b/gtsam/nonlinear/DoglegOptimizerImpl.cpp
@@ -16,6 +16,7 @@
  */
 
 #include <cmath>
+#include <cassert>
 #include <gtsam/nonlinear/DoglegOptimizerImpl.h>
 
 using namespace std;
diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h
index 8ce9b361ed..1038dd522e 100644
--- a/gtsam/nonlinear/DoglegOptimizerImpl.h
+++ b/gtsam/nonlinear/DoglegOptimizerImpl.h
@@ -17,6 +17,7 @@
 #pragma once
 
 #include <iomanip>
+#include <cassert>
 
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/inference/Ordering.h>
diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h
index 71ef1d8405..b09bcb3a5b 100644
--- a/gtsam/nonlinear/Expression-inl.h
+++ b/gtsam/nonlinear/Expression-inl.h
@@ -26,6 +26,7 @@
 #include <set>
 #include <string>
 #include <vector>
+#include <cassert>
 
 
 namespace gtsam {
diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h
index 5bf643c120..072f40b58c 100644
--- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h
+++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h
@@ -23,6 +23,8 @@
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 
+#include <cassert>
+
 namespace gtsam {
 
   /* ************************************************************************* */
diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp
index 20874e2dce..d0b27f3f35 100644
--- a/gtsam/nonlinear/ISAM2-impl.cpp
+++ b/gtsam/nonlinear/ISAM2-impl.cpp
@@ -25,6 +25,7 @@
 #include <functional>
 #include <limits>
 #include <string>
+#include <cassert>
 
 using namespace std;
 
diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h
index e9a9696eb0..bb0210237a 100644
--- a/gtsam/nonlinear/ISAM2-impl.h
+++ b/gtsam/nonlinear/ISAM2-impl.h
@@ -33,6 +33,7 @@
 #include <string>
 #include <utility>
 #include <variant>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp
index 47857a2a2e..0eb2deef54 100644
--- a/gtsam/nonlinear/ISAM2.cpp
+++ b/gtsam/nonlinear/ISAM2.cpp
@@ -29,6 +29,7 @@
 #include <map>
 #include <utility>
 #include <variant>
+#include <cassert>
 
 using namespace std;
 
diff --git a/gtsam/nonlinear/ISAM2Clique.cpp b/gtsam/nonlinear/ISAM2Clique.cpp
index d4b050f841..64ab7c66e3 100644
--- a/gtsam/nonlinear/ISAM2Clique.cpp
+++ b/gtsam/nonlinear/ISAM2Clique.cpp
@@ -22,6 +22,7 @@
 
 #include <stack>
 #include <utility>
+#include <cassert>
 
 using namespace std;
 
diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp
index 7312bf6d96..5eabfac910 100644
--- a/gtsam/nonlinear/NonlinearFactor.cpp
+++ b/gtsam/nonlinear/NonlinearFactor.cpp
@@ -19,6 +19,8 @@
 #include <gtsam/hybrid/HybridValues.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 /* ************************************************************************* */
diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp
index 6af9f2b6a9..1caaa9db16 100644
--- a/gtsam/nonlinear/Values.cpp
+++ b/gtsam/nonlinear/Values.cpp
@@ -28,6 +28,7 @@
 #include <list>
 #include <memory>
 #include <sstream>
+#include <cassert>
 
 using namespace std;
 
diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h
index de0c9721e5..25587f5119 100644
--- a/gtsam/nonlinear/internal/ExpressionNode.h
+++ b/gtsam/nonlinear/internal/ExpressionNode.h
@@ -26,6 +26,7 @@
 #include <typeinfo>       // operator typeid
 #include <ostream>
 #include <map>
+#include <cassert>
 
 class ExpressionFactorBinaryTest;
 // Forward declare for testing
diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp
index 830f1c7197..6c9f91e903 100644
--- a/gtsam/sfm/ShonanAveraging.cpp
+++ b/gtsam/sfm/ShonanAveraging.cpp
@@ -37,6 +37,7 @@
 #include <random>
 #include <set>
 #include <vector>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h
index 5fdf138ccd..3c2f105d46 100644
--- a/gtsam/slam/EssentialMatrixFactor.h
+++ b/gtsam/slam/EssentialMatrixFactor.h
@@ -25,6 +25,7 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
 #include <iostream>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h
index 5d6ec44457..b8e789aaa7 100644
--- a/gtsam/slam/SmartFactorBase.h
+++ b/gtsam/slam/SmartFactorBase.h
@@ -34,6 +34,7 @@
 #include <boost/serialization/optional.hpp>
 #endif
 #include <vector>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam_unstable/discrete/Scheduler.cpp b/gtsam_unstable/discrete/Scheduler.cpp
index 0de4d32c47..53be25d554 100644
--- a/gtsam_unstable/discrete/Scheduler.cpp
+++ b/gtsam_unstable/discrete/Scheduler.cpp
@@ -14,6 +14,7 @@
 #include <cmath>
 #include <fstream>
 #include <iomanip>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam_unstable/dynamics/DynamicsPriors.h b/gtsam_unstable/dynamics/DynamicsPriors.h
index d4ebcba19f..2dc0d552b9 100644
--- a/gtsam_unstable/dynamics/DynamicsPriors.h
+++ b/gtsam_unstable/dynamics/DynamicsPriors.h
@@ -12,6 +12,8 @@
 #include <gtsam_unstable/dynamics/PoseRTV.h>
 #include <gtsam_unstable/slam/PartialPriorFactor.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 // Indices of relevant variables in the PoseRTV tangent vector:
diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h
index 02da899b79..eea019cd2e 100644
--- a/gtsam_unstable/dynamics/FullIMUFactor.h
+++ b/gtsam_unstable/dynamics/FullIMUFactor.h
@@ -10,6 +10,8 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam_unstable/dynamics/PoseRTV.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 /**
diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h
index ee09600e5a..241b6b4974 100644
--- a/gtsam_unstable/dynamics/IMUFactor.h
+++ b/gtsam_unstable/dynamics/IMUFactor.h
@@ -10,6 +10,8 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam_unstable/dynamics/PoseRTV.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 /**
diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp
index cf21c315b9..9457c99e8a 100644
--- a/gtsam_unstable/dynamics/PoseRTV.cpp
+++ b/gtsam_unstable/dynamics/PoseRTV.cpp
@@ -7,6 +7,8 @@
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/base/Vector.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 using namespace std;
diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h
index 2bb70e1b56..dbdd272598 100644
--- a/gtsam_unstable/dynamics/VelocityConstraint.h
+++ b/gtsam_unstable/dynamics/VelocityConstraint.h
@@ -10,6 +10,8 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam_unstable/dynamics/PoseRTV.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 namespace dynamics {
diff --git a/gtsam_unstable/geometry/BearingS2.cpp b/gtsam_unstable/geometry/BearingS2.cpp
index 9dfed612c1..23afd158b0 100644
--- a/gtsam_unstable/geometry/BearingS2.cpp
+++ b/gtsam_unstable/geometry/BearingS2.cpp
@@ -9,6 +9,8 @@
 
 #include <gtsam_unstable/geometry/BearingS2.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 using namespace std;
diff --git a/gtsam_unstable/geometry/Pose3Upright.cpp b/gtsam_unstable/geometry/Pose3Upright.cpp
index 9cd9f78f5e..c5ef0d70f1 100644
--- a/gtsam_unstable/geometry/Pose3Upright.cpp
+++ b/gtsam_unstable/geometry/Pose3Upright.cpp
@@ -6,8 +6,9 @@
  */
 
 #include <iostream>
-#include "gtsam/base/OptionalJacobian.h"
-#include "gtsam/base/Vector.h"
+#include <cassert>
+#include <gtsam/base/OptionalJacobian.h>
+#include <gtsam/base/Vector.h>
 
 #include <gtsam_unstable/geometry/Pose3Upright.h>
 
diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
index 36cb6165fd..8dfb15aa25 100644
--- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
+++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
@@ -21,6 +21,8 @@
 #include <gtsam/base/timing.h>
 #include <gtsam/base/debug.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 /* ************************************************************************* */
diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.cpp b/gtsam_unstable/nonlinear/LinearizedFactor.cpp
index 2d404005d2..375c49341a 100644
--- a/gtsam_unstable/nonlinear/LinearizedFactor.cpp
+++ b/gtsam_unstable/nonlinear/LinearizedFactor.cpp
@@ -17,6 +17,7 @@
 
 #include <gtsam_unstable/nonlinear/LinearizedFactor.h>
 #include <iostream>
+#include <cassert>
 
 namespace gtsam {
 
diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h
index f4718f7a94..21ed91604c 100644
--- a/gtsam_unstable/partition/FindSeparator-inl.h
+++ b/gtsam_unstable/partition/FindSeparator-inl.h
@@ -14,6 +14,7 @@
 #include <iostream>
 #include <vector>
 #include <optional>
+#include <cassert>
 #include <boost/shared_array.hpp>
 
 #include <gtsam/base/timing.h>
diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h
index 7722e5d82b..0a735a2e14 100644
--- a/gtsam_unstable/slam/PartialPriorFactor.h
+++ b/gtsam_unstable/slam/PartialPriorFactor.h
@@ -20,6 +20,8 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/base/Lie.h>
 
+#include <cassert>
+
 namespace gtsam {
 
   /**
diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp
index c4a4731544..6d78bdb7a0 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp
+++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp
@@ -18,6 +18,8 @@
 
 #include <gtsam_unstable/slam/SmartStereoProjectionFactorPP.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 SmartStereoProjectionFactorPP::SmartStereoProjectionFactorPP(
diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp
index 57913385a1..52129daa87 100644
--- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp
+++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp
@@ -21,6 +21,8 @@
 
 #include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 SmartStereoProjectionPoseFactor::SmartStereoProjectionPoseFactor(
diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp
index 721ed51c02..3b507d8065 100644
--- a/tests/testGaussianISAM2.cpp
+++ b/tests/testGaussianISAM2.cpp
@@ -25,6 +25,7 @@
 
 #include <CppUnitLite/TestHarness.h>
 
+#include <cassert>
 
 using namespace std;
 using namespace gtsam;