Cleanup some includes and fix cassert

examples/CombinedImuFactorsExample.cpp

@@ -48,6 +48,7 @@
 #include <cstring>
 #include <fstream>
 #include <iostream>
+#include <cassert>
 
 using namespace gtsam;
 using namespace std;

examples/ImuFactorsExample.cpp

@@ -49,6 +49,7 @@
 #include <gtsam/slam/dataset.h>
 
 #include <cstring>
+#include <cassert>
 #include <fstream>
 #include <iostream>
 

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;
 

gtsam/base/Matrix.cpp

@@ -24,6 +24,7 @@
 #include <Eigen/LU>
 
 #include <cstdarg>
+#include <cassert>
 #include <cstring>
 #include <iomanip>
 #include <list>

gtsam/base/Vector.cpp

@@ -24,6 +24,7 @@
 #include <iostream>
 #include <fstream>
 #include <sstream>
+#include <cassert>
 #include <iomanip>
 #include <cmath>
 #include <cstdio>

gtsam/base/VerticalBlockMatrix.h

@@ -21,6 +21,8 @@
 #include <gtsam/base/MatrixSerialization.h>
 #include <gtsam/base/FastVector.h>
 
+#include <cassert>
+
 namespace gtsam {
 
   // Forward declarations

gtsam/base/cholesky.cpp

@@ -21,6 +21,7 @@
 #include <gtsam/base/timing.h>
 
 #include <cmath>
+#include <cassert>
 
 using namespace std;
 

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;
 

gtsam/basis/Chebyshev2.h

@@ -36,6 +36,8 @@
 #include <gtsam/base/OptionalJacobian.h>
 #include <gtsam/basis/Basis.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 /**

gtsam/discrete/DiscreteConditional.cpp

@@ -30,6 +30,7 @@
 #include <string>
 #include <utility>
 #include <vector>
+#include <cassert>
 
 using namespace std;
 using std::pair;

gtsam/discrete/DiscreteLookupDAG.cpp

@@ -23,6 +23,7 @@
 #include <iterator>
 #include <string>
 #include <utility>
+#include <cassert>
 
 using std::pair;
 using std::vector;

gtsam/geometry/Cal3f.cpp

@@ -28,6 +28,7 @@
 #include <gtsam/geometry/Cal3f.h>
 
 #include <iostream>
+#include <cassert>
 
 namespace gtsam {
 

gtsam/geometry/CameraSet.h

@@ -26,6 +26,7 @@
 #include <gtsam/inference/Key.h>
 
 #include <vector>
+#include <cassert>
 
 namespace gtsam {
 

gtsam/geometry/Pose2.cpp

@@ -20,6 +20,7 @@
 #include <gtsam/base/concepts.h>
 
 #include <cmath>
+#include <cassert>
 #include <iostream>
 #include <iomanip>
 

gtsam/geometry/Pose2.h

@@ -28,6 +28,7 @@
 #include <gtsam/base/std_optional_serialization.h>
 
 #include <optional>
+#include <cassert>
 
 namespace gtsam {
 

gtsam/geometry/Rot3.cpp

@@ -23,6 +23,7 @@
 #include <gtsam/geometry/SO3.h>
 
 #include <cmath>
+#include <cassert>
 #include <random>
 
 using namespace std;

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

gtsam/geometry/SOn.h

@@ -33,6 +33,7 @@
 #include <type_traits>
 #include <vector>
 #include <random>
+#include <cassert>
 
 namespace gtsam {
 

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 {
 

gtsam/hybrid/HybridJunctionTree.cpp

@@ -22,6 +22,7 @@
 #include <gtsam/inference/Key.h>
 
 #include <unordered_map>
+#include <cassert>
 
 namespace gtsam {
 

gtsam/inference/BayesTree-inst.h

@@ -27,7 +27,7 @@
 
 #include <fstream>
 #include <queue>
-
+#include <cassert>
 namespace gtsam {
 
   /* ************************************************************************* */

gtsam/inference/ClusterTree-inst.h

@@ -19,6 +19,7 @@
 #include <mutex>
 #endif
 #include <queue>
+#include <cassert>
 
 namespace gtsam {
 

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>

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>

gtsam/inference/MetisIndex.h

@@ -25,6 +25,7 @@
 #include <vector>
 #include <map>
 #include <unordered_map>
+#include <cassert>
 
 namespace gtsam {
 /**

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>

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;
 

gtsam/linear/HessianFactor.h

@@ -23,6 +23,7 @@
 #include <gtsam/base/SymmetricBlockMatrix.h>
 #include <gtsam/base/FastVector.h>
 
+#include <cassert>
 
 namespace gtsam {
 

gtsam/linear/JacobianFactor.cpp

@@ -32,6 +32,7 @@
 #include <gtsam/base/cholesky.h>
 
 #include <cmath>
+#include <cassert>
 #include <sstream>
 #include <stdexcept>
 

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;
 

gtsam/linear/NoiseModel.cpp

@@ -20,6 +20,7 @@
 #include <gtsam/base/timing.h>
 
 #include <cmath>
+#include <cassert>
 #include <iostream>
 #include <limits>
 #include <stdexcept>

gtsam/linear/Sampler.cpp

@@ -17,6 +17,9 @@
  */
 
 #include <gtsam/linear/Sampler.h>
+
+#include <cassert>
+
 namespace gtsam {
 
 /* ************************************************************************* */

gtsam/linear/SubgraphPreconditioner.cpp

@@ -25,6 +25,7 @@
 #include <gtsam/base/Vector.h>
 
 #include <stdexcept>
+#include <cassert>
 
 using std::cout;
 using std::endl;

gtsam/navigation/ImuFactor.cpp

@@ -23,6 +23,7 @@
 
 /* External or standard includes */
 #include <ostream>
+#include <cassert>
 
 namespace gtsam {
 

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 {

gtsam/nonlinear/DoglegOptimizerImpl.cpp

@@ -16,6 +16,7 @@
  */
 
 #include <cmath>
+#include <cassert>
 #include <gtsam/nonlinear/DoglegOptimizerImpl.h>
 
 using namespace std;

gtsam/nonlinear/DoglegOptimizerImpl.h

@@ -17,6 +17,7 @@
 #pragma once
 
 #include <iomanip>
+#include <cassert>
 
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/inference/Ordering.h>

gtsam/nonlinear/Expression-inl.h

@@ -26,6 +26,7 @@
 #include <set>
 #include <string>
 #include <vector>
+#include <cassert>
 
 
 namespace gtsam {

gtsam/nonlinear/ExtendedKalmanFilter-inl.h

@@ -23,6 +23,8 @@
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 
+#include <cassert>
+
 namespace gtsam {
 
   /* ************************************************************************* */

gtsam/nonlinear/ISAM2-impl.cpp

@@ -25,6 +25,7 @@
 #include <functional>
 #include <limits>
 #include <string>
+#include <cassert>
 
 using namespace std;
 

gtsam/nonlinear/ISAM2-impl.h

@@ -33,6 +33,7 @@
 #include <string>
 #include <utility>
 #include <variant>
+#include <cassert>
 
 namespace gtsam {
 

gtsam/nonlinear/ISAM2.cpp

@@ -29,6 +29,7 @@
 #include <map>
 #include <utility>
 #include <variant>
+#include <cassert>
 
 using namespace std;
 

gtsam/nonlinear/ISAM2Clique.cpp

@@ -22,6 +22,7 @@
 
 #include <stack>
 #include <utility>
+#include <cassert>
 
 using namespace std;
 

gtsam/nonlinear/NonlinearFactor.cpp

@@ -19,6 +19,8 @@
 #include <gtsam/hybrid/HybridValues.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 /* ************************************************************************* */

gtsam/nonlinear/Values.cpp

@@ -28,6 +28,7 @@
 #include <list>
 #include <memory>
 #include <sstream>
+#include <cassert>
 
 using namespace std;
 

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

gtsam/sfm/ShonanAveraging.cpp

@@ -37,6 +37,7 @@
 #include <random>
 #include <set>
 #include <vector>
+#include <cassert>
 
 namespace gtsam {
 

gtsam/slam/EssentialMatrixFactor.h

@@ -25,6 +25,7 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
 #include <iostream>
+#include <cassert>
 
 namespace gtsam {
 

gtsam/slam/SmartFactorBase.h

@@ -34,6 +34,7 @@
 #include <boost/serialization/optional.hpp>
 #endif
 #include <vector>
+#include <cassert>
 
 namespace gtsam {
 

gtsam_unstable/discrete/Scheduler.cpp

@@ -14,6 +14,7 @@
 #include <cmath>
 #include <fstream>
 #include <iomanip>
+#include <cassert>
 
 namespace gtsam {
 

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:

gtsam_unstable/dynamics/FullIMUFactor.h

@@ -10,6 +10,8 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam_unstable/dynamics/PoseRTV.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 /**

gtsam_unstable/dynamics/IMUFactor.h

@@ -10,6 +10,8 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam_unstable/dynamics/PoseRTV.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 /**

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;

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 {

gtsam_unstable/geometry/BearingS2.cpp

@@ -9,6 +9,8 @@
 
 #include <gtsam_unstable/geometry/BearingS2.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 using namespace std;

gtsam_unstable/geometry/Pose3Upright.cpp

@@ -6,8 +6,9 @@
  */
 
 #include <iostream>
-#include "gtsam/base/OptionalJacobian.h"
+#include <cassert>
-#include "gtsam/base/Vector.h"
+#include <gtsam/base/OptionalJacobian.h>
+#include <gtsam/base/Vector.h>
 
 #include <gtsam_unstable/geometry/Pose3Upright.h>
 

gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp

@@ -21,6 +21,8 @@
 #include <gtsam/base/timing.h>
 #include <gtsam/base/debug.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 /* ************************************************************************* */

gtsam_unstable/nonlinear/LinearizedFactor.cpp

@@ -17,6 +17,7 @@
 
 #include <gtsam_unstable/nonlinear/LinearizedFactor.h>
 #include <iostream>
+#include <cassert>
 
 namespace gtsam {
 

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>

gtsam_unstable/slam/PartialPriorFactor.h

@@ -20,6 +20,8 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/base/Lie.h>
 
+#include <cassert>
+
 namespace gtsam {
 
   /**

gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp

@@ -18,6 +18,8 @@
 
 #include <gtsam_unstable/slam/SmartStereoProjectionFactorPP.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 SmartStereoProjectionFactorPP::SmartStereoProjectionFactorPP(

gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp

@@ -21,6 +21,8 @@
 
 #include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
 
+#include <cassert>
+
 namespace gtsam {
 
 SmartStereoProjectionPoseFactor::SmartStereoProjectionPoseFactor(

tests/testGaussianISAM2.cpp

@@ -25,6 +25,7 @@
 
 #include <CppUnitLite/TestHarness.h>
 
+#include <cassert>
 
 using namespace std;
 using namespace gtsam;