Merge pull request #1107 from borglab/fix/91_single_test_exe

.github/scripts/unix.sh

@@ -71,6 +71,7 @@ function configure()
       -DGTSAM_USE_SYSTEM_EIGEN=${GTSAM_USE_SYSTEM_EIGEN:-OFF} \
       -DGTSAM_USE_SYSTEM_METIS=${GTSAM_USE_SYSTEM_METIS:-OFF} \
       -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
+      -DGTSAM_SINGLE_TEST_EXE=ON \
       -DBOOST_ROOT=$BOOST_ROOT \
       -DBoost_NO_SYSTEM_PATHS=ON \
       -DBoost_ARCHITECTURE=-x64

examples/Data/randomGrid3D.xml

@@ -7,7 +7,7 @@
 			<count>32</count>
 			<item_version>1</item_version>
 			<item class_id="3" tracking_level="0" version="1">
-				<px class_id="4" class_name="JacobianFactor" tracking_level="1" version="0" object_id="_0">
+				<px class_id="4" class_name="gtsam::JacobianFactor" tracking_level="1" version="0" object_id="_0">
 					<Base class_id="5" tracking_level="0" version="0">
 						<Base class_id="6" tracking_level="0" version="0">
 							<keys_>

examples/Data/toy3D.xml

@@ -7,7 +7,7 @@
 			<count>2</count>
 			<item_version>1</item_version>
 			<item class_id="3" tracking_level="0" version="1">
-				<px class_id="4" class_name="JacobianFactor" tracking_level="1" version="0" object_id="_0">
+				<px class_id="4" class_name="gtsam::JacobianFactor" tracking_level="1" version="0" object_id="_0">
 					<Base class_id="5" tracking_level="0" version="0">
 						<Base class_id="6" tracking_level="0" version="0">
 							<keys_>

gtsam/base/FastSet.h

@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include <boost/version.hpp>
 #if BOOST_VERSION >= 107400
 #include <boost/serialization/library_version_type.hpp>
 #endif

gtsam/base/serializationTestHelpers.h

@@ -42,7 +42,7 @@ T create() {
 }
 
 // Creates or empties a folder in the build folder and returns the relative path
-boost::filesystem::path resetFilesystem(
+inline boost::filesystem::path resetFilesystem(
     boost::filesystem::path folder = "actual") {
   boost::filesystem::remove_all(folder);
   boost::filesystem::create_directory(folder);

gtsam/basis/tests/testChebyshev.cpp

@@ -25,9 +25,10 @@
 using namespace std;
 using namespace gtsam;
 
+namespace {
 auto model = noiseModel::Unit::Create(1);
-
 const size_t N = 3;
+}  // namespace
 
 //******************************************************************************
 TEST(Chebyshev, Chebyshev1) {

gtsam/basis/tests/testChebyshev2.cpp

@@ -27,9 +27,11 @@ using namespace std;
 using namespace gtsam;
 using namespace boost::placeholders;
 
+namespace {
 noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1);
 
 const size_t N = 32;
+}  // namespace
 
 //******************************************************************************
 TEST(Chebyshev2, Point) {

gtsam/discrete/tests/testDecisionTree.cpp

@@ -17,17 +17,19 @@
  * @date    Jan 30, 2012
  */
 
-#include <boost/assign/std/vector.hpp>
-using namespace boost::assign;
-
-#include <CppUnitLite/TestHarness.h>
-#include <gtsam/base/Testable.h>
-#include <gtsam/discrete/Signature.h>
-
 // #define DT_DEBUG_MEMORY
 // #define DT_NO_PRUNING
 #define DISABLE_DOT
 #include <gtsam/discrete/DecisionTree-inl.h>
+
+#include <gtsam/base/Testable.h>
+#include <gtsam/discrete/Signature.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <boost/assign/std/vector.hpp>
+using namespace boost::assign;
+
 using namespace std;
 using namespace gtsam;
 
@@ -148,9 +150,9 @@ TEST(DecisionTree, example) {
   DOT(notb);
 
   // Check supplying empty trees yields an exception
-  CHECK_EXCEPTION(apply(empty, &Ring::id), std::runtime_error);
+  CHECK_EXCEPTION(gtsam::apply(empty, &Ring::id), std::runtime_error);
-  CHECK_EXCEPTION(apply(empty, a, &Ring::mul), std::runtime_error);
+  CHECK_EXCEPTION(gtsam::apply(empty, a, &Ring::mul), std::runtime_error);
-  CHECK_EXCEPTION(apply(a, empty, &Ring::mul), std::runtime_error);
+  CHECK_EXCEPTION(gtsam::apply(a, empty, &Ring::mul), std::runtime_error);
 
   // apply, two nodes, in natural order
   DT anotb = apply(a, notb, &Ring::mul);

gtsam/discrete/tests/testDecisionTreeFactor.cpp

@@ -107,7 +107,7 @@ TEST(DecisionTreeFactor, enumerate) {
 }
 
 /* ************************************************************************* */
-TEST(DiscreteFactorGraph, DotWithNames) {
+TEST(DecisionTreeFactor, DotWithNames) {
   DiscreteKey A(12, 3), B(5, 2);
   DecisionTreeFactor f(A & B, "1 2  3 4  5 6");
   auto formatter = [](Key key) { return key == 12 ? "A" : "B"; };

gtsam/geometry/PinholeCamera.h

@@ -230,13 +230,15 @@ public:
   Point2 _project2(const POINT& pw, OptionalJacobian<2, dimension> Dcamera,
       OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint) const {
     // We just call 3-derivative version in Base
-    Matrix26 Dpose;
+    if (Dcamera){
-    Eigen::Matrix<double, 2, DimK> Dcal;
+      Matrix26 Dpose;
-    Point2 pi = Base::project(pw, Dcamera ? &Dpose : 0, Dpoint,
+      Eigen::Matrix<double, 2, DimK> Dcal;
-        Dcamera ? &Dcal : 0);
+      const Point2 pi = Base::project(pw, Dpose, Dpoint, Dcal);
-    if (Dcamera)
       *Dcamera << Dpose, Dcal;
-    return pi;
+      return pi;
+    } else {
+      return Base::project(pw, boost::none, Dpoint, boost::none);
+    }
   }
 
   /// project a 3D point from world coordinates into the image

gtsam/geometry/tests/testCal3Bundler.cpp

@@ -160,7 +160,7 @@ TEST(Cal3Bundler, retract) {
 }
 
 /* ************************************************************************* */
-TEST(Cal3_S2, Print) {
+TEST(Cal3Bundler, Print) {
   Cal3Bundler cal(1, 2, 3, 4, 5);
   std::stringstream os;
   os << "f: " << cal.fx() << ", k1: " << cal.k1() << ", k2: " << cal.k2()

gtsam/geometry/tests/testPose2.cpp

@@ -796,44 +796,39 @@ TEST(Pose2, align_4) {
 }
 
 //******************************************************************************
+namespace {
+Pose2 id;
 Pose2 T1(M_PI / 4.0, Point2(sqrt(0.5), sqrt(0.5)));
 Pose2 T2(M_PI / 2.0, Point2(0.0, 2.0));
+}  // namespace
 
 //******************************************************************************
-TEST(Pose2 , Invariants) {
+TEST(Pose2, Invariants) {
-  Pose2 id;
+  EXPECT(check_group_invariants(id, id));
-
+  EXPECT(check_group_invariants(id, T1));
-  EXPECT(check_group_invariants(id,id));
+  EXPECT(check_group_invariants(T2, id));
-  EXPECT(check_group_invariants(id,T1));
+  EXPECT(check_group_invariants(T2, T1));
-  EXPECT(check_group_invariants(T2,id));
-  EXPECT(check_group_invariants(T2,T1));
-
-  EXPECT(check_manifold_invariants(id,id));
-  EXPECT(check_manifold_invariants(id,T1));
-  EXPECT(check_manifold_invariants(T2,id));
-  EXPECT(check_manifold_invariants(T2,T1));
 
+  EXPECT(check_manifold_invariants(id, id));
+  EXPECT(check_manifold_invariants(id, T1));
+  EXPECT(check_manifold_invariants(T2, id));
+  EXPECT(check_manifold_invariants(T2, T1));
 }
 
 //******************************************************************************
-TEST(Pose2 , LieGroupDerivatives) {
+TEST(Pose2, LieGroupDerivatives) {
-  Pose2 id;
+  CHECK_LIE_GROUP_DERIVATIVES(id, id);
-
+  CHECK_LIE_GROUP_DERIVATIVES(id, T2);
-  CHECK_LIE_GROUP_DERIVATIVES(id,id);
+  CHECK_LIE_GROUP_DERIVATIVES(T2, id);
-  CHECK_LIE_GROUP_DERIVATIVES(id,T2);
+  CHECK_LIE_GROUP_DERIVATIVES(T2, T1);
-  CHECK_LIE_GROUP_DERIVATIVES(T2,id);
-  CHECK_LIE_GROUP_DERIVATIVES(T2,T1);
-
 }
 
 //******************************************************************************
-TEST(Pose2 , ChartDerivatives) {
+TEST(Pose2, ChartDerivatives) {
-  Pose2 id;
+  CHECK_CHART_DERIVATIVES(id, id);
-
+  CHECK_CHART_DERIVATIVES(id, T2);
-  CHECK_CHART_DERIVATIVES(id,id);
+  CHECK_CHART_DERIVATIVES(T2, id);
-  CHECK_CHART_DERIVATIVES(id,T2);
+  CHECK_CHART_DERIVATIVES(T2, T1);
-  CHECK_CHART_DERIVATIVES(T2,id);
-  CHECK_CHART_DERIVATIVES(T2,T1);
 }
 
 //******************************************************************************

gtsam/geometry/tests/testQuaternion.cpp

@@ -80,12 +80,6 @@ TEST(Quaternion , Compose) {
   EXPECT(traits<Q>::Equals(expected, actual));
 }
 
-//******************************************************************************
-Vector3 Q_z_axis(0, 0, 1);
-Q id(Eigen::AngleAxisd(0, Q_z_axis));
-Q R1(Eigen::AngleAxisd(1, Q_z_axis));
-Q R2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)));
-
 //******************************************************************************
 TEST(Quaternion , Between) {
   Vector3 z_axis(0, 0, 1);
@@ -108,7 +102,15 @@ TEST(Quaternion , Inverse) {
 }
 
 //******************************************************************************
-TEST(Quaternion , Invariants) {
+namespace {
+Vector3 Q_z_axis(0, 0, 1);
+Q id(Eigen::AngleAxisd(0, Q_z_axis));
+Q R1(Eigen::AngleAxisd(1, Q_z_axis));
+Q R2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)));
+}  // namespace
+
+//******************************************************************************
+TEST(Quaternion, Invariants) {
   EXPECT(check_group_invariants(id, id));
   EXPECT(check_group_invariants(id, R1));
   EXPECT(check_group_invariants(R2, id));
@@ -121,7 +123,7 @@ TEST(Quaternion , Invariants) {
 }
 
 //******************************************************************************
-TEST(Quaternion , LieGroupDerivatives) {
+TEST(Quaternion, LieGroupDerivatives) {
   CHECK_LIE_GROUP_DERIVATIVES(id, id);
   CHECK_LIE_GROUP_DERIVATIVES(id, R2);
   CHECK_LIE_GROUP_DERIVATIVES(R2, id);
@@ -129,7 +131,7 @@ TEST(Quaternion , LieGroupDerivatives) {
 }
 
 //******************************************************************************
-TEST(Quaternion , ChartDerivatives) {
+TEST(Quaternion, ChartDerivatives) {
   CHECK_CHART_DERIVATIVES(id, id);
   CHECK_CHART_DERIVATIVES(id, R2);
   CHECK_CHART_DERIVATIVES(R2, id);

gtsam/geometry/tests/testRot2.cpp

@@ -156,44 +156,39 @@ TEST( Rot2, relativeBearing )
 }
 
 //******************************************************************************
+namespace {
+Rot2 id;
 Rot2 T1(0.1);
 Rot2 T2(0.2);
+}  // namespace
 
 //******************************************************************************
-TEST(Rot2 , Invariants) {
+TEST(Rot2, Invariants) {
-  Rot2 id;
+  EXPECT(check_group_invariants(id, id));
-
+  EXPECT(check_group_invariants(id, T1));
-  EXPECT(check_group_invariants(id,id));
+  EXPECT(check_group_invariants(T2, id));
-  EXPECT(check_group_invariants(id,T1));
+  EXPECT(check_group_invariants(T2, T1));
-  EXPECT(check_group_invariants(T2,id));
-  EXPECT(check_group_invariants(T2,T1));
-
-  EXPECT(check_manifold_invariants(id,id));
-  EXPECT(check_manifold_invariants(id,T1));
-  EXPECT(check_manifold_invariants(T2,id));
-  EXPECT(check_manifold_invariants(T2,T1));
 
+  EXPECT(check_manifold_invariants(id, id));
+  EXPECT(check_manifold_invariants(id, T1));
+  EXPECT(check_manifold_invariants(T2, id));
+  EXPECT(check_manifold_invariants(T2, T1));
 }
 
 //******************************************************************************
-TEST(Rot2 , LieGroupDerivatives) {
+TEST(Rot2, LieGroupDerivatives) {
-  Rot2 id;
+  CHECK_LIE_GROUP_DERIVATIVES(id, id);
-
+  CHECK_LIE_GROUP_DERIVATIVES(id, T2);
-  CHECK_LIE_GROUP_DERIVATIVES(id,id);
+  CHECK_LIE_GROUP_DERIVATIVES(T2, id);
-  CHECK_LIE_GROUP_DERIVATIVES(id,T2);
+  CHECK_LIE_GROUP_DERIVATIVES(T2, T1);
-  CHECK_LIE_GROUP_DERIVATIVES(T2,id);
-  CHECK_LIE_GROUP_DERIVATIVES(T2,T1);
-
 }
 
 //******************************************************************************
-TEST(Rot2 , ChartDerivatives) {
+TEST(Rot2, ChartDerivatives) {
-  Rot2 id;
+  CHECK_CHART_DERIVATIVES(id, id);
-
+  CHECK_CHART_DERIVATIVES(id, T2);
-  CHECK_CHART_DERIVATIVES(id,id);
+  CHECK_CHART_DERIVATIVES(T2, id);
-  CHECK_CHART_DERIVATIVES(id,T2);
+  CHECK_CHART_DERIVATIVES(T2, T1);
-  CHECK_CHART_DERIVATIVES(T2,id);
-  CHECK_CHART_DERIVATIVES(T2,T1);
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testRot3.cpp

@@ -640,46 +640,44 @@ TEST( Rot3, slerp)
 }
 
 //******************************************************************************
+namespace {
+Rot3 id;
 Rot3 T1(Rot3::AxisAngle(Vector3(0, 0, 1), 1));
 Rot3 T2(Rot3::AxisAngle(Vector3(0, 1, 0), 2));
+}  // namespace
 
 //******************************************************************************
-TEST(Rot3 , Invariants) {
+TEST(Rot3, Invariants) {
-  Rot3 id;
+  EXPECT(check_group_invariants(id, id));
+  EXPECT(check_group_invariants(id, T1));
+  EXPECT(check_group_invariants(T2, id));
+  EXPECT(check_group_invariants(T2, T1));
+  EXPECT(check_group_invariants(T1, T2));
 
-  EXPECT(check_group_invariants(id,id));
+  EXPECT(check_manifold_invariants(id, id));
-  EXPECT(check_group_invariants(id,T1));
+  EXPECT(check_manifold_invariants(id, T1));
-  EXPECT(check_group_invariants(T2,id));
+  EXPECT(check_manifold_invariants(T2, id));
-  EXPECT(check_group_invariants(T2,T1));
+  EXPECT(check_manifold_invariants(T2, T1));
-  EXPECT(check_group_invariants(T1,T2));
+  EXPECT(check_manifold_invariants(T1, T2));
-
-  EXPECT(check_manifold_invariants(id,id));
-  EXPECT(check_manifold_invariants(id,T1));
-  EXPECT(check_manifold_invariants(T2,id));
-  EXPECT(check_manifold_invariants(T2,T1));
-  EXPECT(check_manifold_invariants(T1,T2));
 }
 
 //******************************************************************************
-TEST(Rot3 , LieGroupDerivatives) {
+TEST(Rot3, LieGroupDerivatives) {
-  Rot3 id;
+  CHECK_LIE_GROUP_DERIVATIVES(id, id);
-
+  CHECK_LIE_GROUP_DERIVATIVES(id, T2);
-  CHECK_LIE_GROUP_DERIVATIVES(id,id);
+  CHECK_LIE_GROUP_DERIVATIVES(T2, id);
-  CHECK_LIE_GROUP_DERIVATIVES(id,T2);
+  CHECK_LIE_GROUP_DERIVATIVES(T1, T2);
-  CHECK_LIE_GROUP_DERIVATIVES(T2,id);
+  CHECK_LIE_GROUP_DERIVATIVES(T2, T1);
-  CHECK_LIE_GROUP_DERIVATIVES(T1,T2);
-  CHECK_LIE_GROUP_DERIVATIVES(T2,T1);
 }
 
 //******************************************************************************
-TEST(Rot3 , ChartDerivatives) {
+TEST(Rot3, ChartDerivatives) {
-  Rot3 id;
   if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) {
-    CHECK_CHART_DERIVATIVES(id,id);
+    CHECK_CHART_DERIVATIVES(id, id);
-    CHECK_CHART_DERIVATIVES(id,T2);
+    CHECK_CHART_DERIVATIVES(id, T2);
-    CHECK_CHART_DERIVATIVES(T2,id);
+    CHECK_CHART_DERIVATIVES(T2, id);
-    CHECK_CHART_DERIVATIVES(T1,T2);
+    CHECK_CHART_DERIVATIVES(T1, T2);
-    CHECK_CHART_DERIVATIVES(T2,T1);
+    CHECK_CHART_DERIVATIVES(T2, T1);
   }
 }
 

gtsam/geometry/tests/testSO3.cpp

@@ -67,10 +67,12 @@ TEST(SO3, ClosestTo) {
 }
 
 //******************************************************************************
+namespace {
 SO3 id;
 Vector3 z_axis(0, 0, 1), v2(1, 2, 0), v3(1, 2, 3);
 SO3 R1(Eigen::AngleAxisd(0.1, z_axis));
 SO3 R2(Eigen::AngleAxisd(0.2, z_axis));
+}  // namespace
 
 /* ************************************************************************* */
 TEST(SO3, ChordalMean) {
@@ -79,16 +81,16 @@ TEST(SO3, ChordalMean) {
 }
 
 //******************************************************************************
+// Check that Hat specialization is equal to dynamic version
 TEST(SO3, Hat) {
-  // Check that Hat specialization is equal to dynamic version
   EXPECT(assert_equal(SO3::Hat(z_axis), SOn::Hat(z_axis)));
   EXPECT(assert_equal(SO3::Hat(v2), SOn::Hat(v2)));
   EXPECT(assert_equal(SO3::Hat(v3), SOn::Hat(v3)));
 }
 
 //******************************************************************************
+// Check that Hat specialization is equal to dynamic version
 TEST(SO3, Vee) {
-  // Check that Hat specialization is equal to dynamic version
   auto X1 = SOn::Hat(z_axis), X2 = SOn::Hat(v2), X3 = SOn::Hat(v3);
   EXPECT(assert_equal(SO3::Vee(X1), SOn::Vee(X1)));
   EXPECT(assert_equal(SO3::Vee(X2), SOn::Vee(X2)));

gtsam/geometry/tests/testSO4.cpp

@@ -48,6 +48,14 @@ TEST(SO4, Concept) {
 }
 
 //******************************************************************************
+TEST(SO4, Random) {
+  std::mt19937 rng(42);
+  auto Q = SO4::Random(rng);
+  EXPECT_LONGS_EQUAL(4, Q.matrix().rows());
+}
+
+//******************************************************************************
+namespace {
 SO4 id;
 Vector6 v1 = (Vector(6) << 0, 0, 0, 0.1, 0, 0).finished();
 SO4 Q1 = SO4::Expmap(v1);
@@ -55,13 +63,8 @@ Vector6 v2 = (Vector(6) << 0.00, 0.00, 0.00, 0.01, 0.02, 0.03).finished();
 SO4 Q2 = SO4::Expmap(v2);
 Vector6 v3 = (Vector(6) << 1, 2, 3, 4, 5, 6).finished();
 SO4 Q3 = SO4::Expmap(v3);
+}  // namespace
 
-//******************************************************************************
-TEST(SO4, Random) {
-  std::mt19937 rng(42);
-  auto Q = SO4::Random(rng);
-  EXPECT_LONGS_EQUAL(4, Q.matrix().rows());
-}
 //******************************************************************************
 TEST(SO4, Expmap) {
   // If we do exponential map in SO(3) subgroup, topleft should be equal to R1.
@@ -84,16 +87,16 @@ TEST(SO4, Expmap) {
 }
 
 //******************************************************************************
+// Check that Hat specialization is equal to dynamic version
 TEST(SO4, Hat) {
-  // Check that Hat specialization is equal to dynamic version
   EXPECT(assert_equal(SO4::Hat(v1), SOn::Hat(v1)));
   EXPECT(assert_equal(SO4::Hat(v2), SOn::Hat(v2)));
   EXPECT(assert_equal(SO4::Hat(v3), SOn::Hat(v3)));
 }
 
 //******************************************************************************
+// Check that Hat specialization is equal to dynamic version
 TEST(SO4, Vee) {
-  // Check that Hat specialization is equal to dynamic version
   auto X1 = SOn::Hat(v1), X2 = SOn::Hat(v2), X3 = SOn::Hat(v3);
   EXPECT(assert_equal(SO4::Vee(X1), SOn::Vee(X1)));
   EXPECT(assert_equal(SO4::Vee(X2), SOn::Vee(X2)));
@@ -116,8 +119,8 @@ TEST(SO4, Retract) {
 }
 
 //******************************************************************************
+// Check that Cayley is identical to dynamic version
 TEST(SO4, Local) {
-  // Check that Cayley is identical to dynamic version
   EXPECT(
       assert_equal(id.localCoordinates(Q1), SOn(4).localCoordinates(SOn(Q1))));
   EXPECT(
@@ -166,9 +169,7 @@ TEST(SO4, vec) {
   Matrix actualH;
   const Vector16 actual = Q2.vec(actualH);
   EXPECT(assert_equal(expected, actual));
-  std::function<Vector16(const SO4&)> f = [](const SO4& Q) {
+  std::function<Vector16(const SO4&)> f = [](const SO4& Q) { return Q.vec(); };
-    return Q.vec();
-  };
   const Matrix numericalH = numericalDerivative11(f, Q2, 1e-5);
   EXPECT(assert_equal(numericalH, actualH));
 }

gtsam/navigation/tests/imuFactorTesting.h

@@ -28,6 +28,7 @@ using symbol_shorthand::X;
 using symbol_shorthand::V;
 using symbol_shorthand::B;
 
+namespace {
 static const Vector3 kZero = Z_3x1;
 typedef imuBias::ConstantBias Bias;
 static const Bias kZeroBiasHat, kZeroBias;
@@ -43,6 +44,7 @@ static const Vector3 kGravityAlongNavZDown(0, 0, kGravity);
 auto radians = [](double t) { return t * M_PI / 180; };
 static const double kGyroSigma = radians(0.5) / 60;  // 0.5 degree ARW
 static const double kAccelSigma = 0.1 / 60;          // 10 cm VRW
+}
 
 namespace testing {
 

gtsam/navigation/tests/testAttitudeFactor.cpp

@@ -19,8 +19,6 @@
 #include <gtsam/navigation/AttitudeFactor.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
-#include <gtsam/base/serialization.h>
-#include <gtsam/base/serializationTestHelpers.h>
 
 #include <boost/bind/bind.hpp>
 #include <CppUnitLite/TestHarness.h>
@@ -63,22 +61,6 @@ TEST( Rot3AttitudeFactor, Constructor ) {
   EXPECT(assert_equal(expectedH, actualH, 1e-8));
 }
 
-/* ************************************************************************* */
-// Export Noisemodels
-// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
-BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic)
-
-/* ************************************************************************* */
-TEST(Rot3AttitudeFactor, Serialization) {
-  Unit3 nDown(0, 0, -1);
-  SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
-  Rot3AttitudeFactor factor(0, nDown, model);
-
-  EXPECT(serializationTestHelpers::equalsObj(factor));
-  EXPECT(serializationTestHelpers::equalsXML(factor));
-  EXPECT(serializationTestHelpers::equalsBinary(factor));
-}
-
 /* ************************************************************************* */
 TEST(Rot3AttitudeFactor, CopyAndMove) {
   Unit3 nDown(0, 0, -1);
@@ -129,17 +111,6 @@ TEST( Pose3AttitudeFactor, Constructor ) {
   EXPECT(assert_equal(expectedH, actualH, 1e-8));
 }
 
-/* ************************************************************************* */
-TEST(Pose3AttitudeFactor, Serialization) {
-  Unit3 nDown(0, 0, -1);
-  SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
-  Pose3AttitudeFactor factor(0, nDown, model);
-
-  EXPECT(serializationTestHelpers::equalsObj(factor));
-  EXPECT(serializationTestHelpers::equalsXML(factor));
-  EXPECT(serializationTestHelpers::equalsBinary(factor));
-}
-
 /* ************************************************************************* */
 TEST(Pose3AttitudeFactor, CopyAndMove) {
   Unit3 nDown(0, 0, -1);

gtsam/navigation/tests/testMagFactor.cpp

@@ -31,7 +31,7 @@ using namespace std;
 using namespace gtsam;
 using namespace GeographicLib;
 
-// *************************************************************************
+namespace {
 // Convert from Mag to ENU
 // ENU Origin is where the plane was in hold next to runway
 // const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274;
@@ -51,10 +51,11 @@ Point3 bias(10, -10, 50);
 Point3 scaled = scale * nM;
 Point3 measured = nRb.inverse() * (scale * nM) + bias;
 
-double s(scale * nM.norm());
+double s(scale* nM.norm());
 Unit3 dir(nM);
 
 SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
+}  // namespace
 
 using boost::none;
 

gtsam/navigation/tests/testMagPoseFactor.cpp

@@ -20,7 +20,7 @@
 using namespace std::placeholders;
 using namespace gtsam;
 
-// *****************************************************************************
+namespace {
 // Magnetic field in the nav frame (NED), with units of nT.
 Point3 nM(22653.29982, -1956.83010, 44202.47862);
 
@@ -51,8 +51,9 @@ SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.25);
 
 // Make up a rotation and offset of the sensor in the body frame.
 Pose2 body_P2_sensor(Rot2(-0.30), Point2(1.0, -2.0));
-Pose3 body_P3_sensor(Rot3::RzRyRx(Vector3(1.5, 0.9, -1.15)), Point3(-0.1, 0.2, 0.3));
+Pose3 body_P3_sensor(Rot3::RzRyRx(Vector3(1.5, 0.9, -1.15)),
-// *****************************************************************************
+                     Point3(-0.1, 0.2, 0.3));
+}  // namespace
 
 // *****************************************************************************
 TEST(MagPoseFactor, Constructors) {

gtsam/navigation/tests/testSerializationNavigation.cpp

@@ -10,17 +10,19 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file    testImuFactor.cpp
+ * @file    testSerializationNavigation.cpp
- * @brief   Unit test for ImuFactor
+ * @brief   serialization tests for navigation
  * @author  Luca Carlone
  * @author  Frank Dellaert
  * @author  Richard Roberts
  * @author  Stephen Williams
  * @author  Varun Agrawal
+ * @date    February 2022
  */
 
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/serializationTestHelpers.h>
+#include <gtsam/navigation/AttitudeFactor.h>
 #include <gtsam/navigation/CombinedImuFactor.h>
 #include <gtsam/navigation/ImuFactor.h>
 
@@ -30,17 +32,13 @@ using namespace std;
 using namespace gtsam;
 using namespace gtsam::serializationTestHelpers;
 
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained,
+BOOST_CLASS_EXPORT_GUID(noiseModel::Constrained, "gtsam_noiseModel_Constrained")
-                        "gtsam_noiseModel_Constrained")
+BOOST_CLASS_EXPORT_GUID(noiseModel::Diagonal, "gtsam_noiseModel_Diagonal")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal,
+BOOST_CLASS_EXPORT_GUID(noiseModel::Gaussian, "gtsam_noiseModel_Gaussian")
-                        "gtsam_noiseModel_Diagonal")
+BOOST_CLASS_EXPORT_GUID(noiseModel::Unit, "gtsam_noiseModel_Unit")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian,
+BOOST_CLASS_EXPORT_GUID(noiseModel::Isotropic, "gtsam_noiseModel_Isotropic")
-                        "gtsam_noiseModel_Gaussian")
+BOOST_CLASS_EXPORT_GUID(SharedNoiseModel, "gtsam_SharedNoiseModel")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit")
+BOOST_CLASS_EXPORT_GUID(SharedDiagonal, "gtsam_SharedDiagonal")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,
-                        "gtsam_noiseModel_Isotropic")
-BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel")
-BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal")
 
 template <typename P>
 P getPreintegratedMeasurements() {
@@ -69,6 +67,7 @@ P getPreintegratedMeasurements() {
   return pim;
 }
 
+/* ************************************************************************* */
 TEST(ImuFactor, serialization) {
   auto pim = getPreintegratedMeasurements<PreintegratedImuMeasurements>();
 
@@ -83,6 +82,7 @@ TEST(ImuFactor, serialization) {
   EXPECT(equalsBinary<ImuFactor>(factor));
 }
 
+/* ************************************************************************* */
 TEST(ImuFactor2, serialization) {
   auto pim = getPreintegratedMeasurements<PreintegratedImuMeasurements>();
 
@@ -93,6 +93,7 @@ TEST(ImuFactor2, serialization) {
   EXPECT(equalsBinary<ImuFactor2>(factor));
 }
 
+/* ************************************************************************* */
 TEST(CombinedImuFactor, Serialization) {
   auto pim = getPreintegratedMeasurements<PreintegratedCombinedMeasurements>();
 
@@ -107,6 +108,28 @@ TEST(CombinedImuFactor, Serialization) {
   EXPECT(equalsBinary<CombinedImuFactor>(factor));
 }
 
+/* ************************************************************************* */
+TEST(Rot3AttitudeFactor, Serialization) {
+  Unit3 nDown(0, 0, -1);
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
+  Rot3AttitudeFactor factor(0, nDown, model);
+
+  EXPECT(serializationTestHelpers::equalsObj(factor));
+  EXPECT(serializationTestHelpers::equalsXML(factor));
+  EXPECT(serializationTestHelpers::equalsBinary(factor));
+}
+
+/* ************************************************************************* */
+TEST(Pose3AttitudeFactor, Serialization) {
+  Unit3 nDown(0, 0, -1);
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
+  Pose3AttitudeFactor factor(0, nDown, model);
+
+  EXPECT(serializationTestHelpers::equalsObj(factor));
+  EXPECT(serializationTestHelpers::equalsXML(factor));
+  EXPECT(serializationTestHelpers::equalsBinary(factor));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/nonlinear/factorTesting.h

@@ -21,6 +21,8 @@
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/base/numericalDerivative.h>
+#include <string>
+#include <vector>
 
 namespace gtsam {
 
@@ -34,13 +36,14 @@ namespace gtsam {
  * This is fixable but expensive, and does not matter in practice as most factors will sit near
  * zero errors anyway. However, it means that below will only be exact for the correct measurement.
  */
-JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
+inline JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
-                                    const Values& values, double delta = 1e-5) {
+                                           const Values& values,
+                                           double delta = 1e-5) {
   // We will fill a vector of key/Jacobians pairs (a map would sort)
   std::vector<std::pair<Key, Matrix> > jacobians;
 
   // Get size
-  const Eigen::VectorXd e = factor.whitenedError(values);
+  const Vector e = factor.whitenedError(values);
   const size_t rows = e.size();
 
   // Loop over all variables
@@ -51,18 +54,18 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
     const size_t cols = dX.dim(key);
     Matrix J = Matrix::Zero(rows, cols);
     for (size_t col = 0; col < cols; ++col) {
-      Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols);
+      Vector dx = Vector::Zero(cols);
       dx(col) = delta;
       dX[key] = dx;
       Values eval_values = values.retract(dX);
-      const Eigen::VectorXd left = factor.whitenedError(eval_values);
+      const Vector left = factor.whitenedError(eval_values);
       dx(col) = -delta;
       dX[key] = dx;
       eval_values = values.retract(dX);
-      const Eigen::VectorXd right = factor.whitenedError(eval_values);
+      const Vector right = factor.whitenedError(eval_values);
       J.col(col) = (left - right) * one_over_2delta;
     }
-    jacobians.push_back(std::make_pair(key, J));
+    jacobians.emplace_back(key, J);
   }
 
   // Next step...return JacobianFactor
@@ -71,15 +74,15 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
 
 namespace internal {
 // CPPUnitLite-style test for linearization of a factor
-bool testFactorJacobians(const std::string& name_,
+inline bool testFactorJacobians(const std::string& name_,
-    const NoiseModelFactor& factor, const gtsam::Values& values, double delta,
+                                const NoiseModelFactor& factor,
-    double tolerance) {
+                                const gtsam::Values& values, double delta,
-
+                                double tolerance) {
   // Create expected value by numerical differentiation
   JacobianFactor expected = linearizeNumerically(factor, values, delta);
 
   // Create actual value by linearize
-  boost::shared_ptr<JacobianFactor> actual = //
+  auto actual =
       boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(values));
   if (!actual) return false;
 
@@ -89,17 +92,19 @@ bool testFactorJacobians(const std::string& name_,
   // if not equal, test individual jacobians:
   if (!equal) {
     for (size_t i = 0; i < actual->size(); i++) {
-      bool i_good = assert_equal((Matrix) (expected.getA(expected.begin() + i)),
+      bool i_good =
-          (Matrix) (actual->getA(actual->begin() + i)), tolerance);
+          assert_equal((Matrix)(expected.getA(expected.begin() + i)),
+                       (Matrix)(actual->getA(actual->begin() + i)), tolerance);
       if (!i_good) {
-        std::cout << "Mismatch in Jacobian " << i+1 << " (base 1), as shown above" << std::endl;
+        std::cout << "Mismatch in Jacobian " << i + 1
+                  << " (base 1), as shown above" << std::endl;
       }
     }
   }
 
   return equal;
 }
-}
+}  // namespace internal
 
 /// \brief Check the Jacobians produced by a factor against finite differences.
 /// \param factor The factor to test.
@@ -109,4 +114,4 @@ bool testFactorJacobians(const std::string& name_,
 #define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance) \
     { EXPECT(gtsam::internal::testFactorJacobians(name_, factor, values, numerical_derivative_step, tolerance)); }
 
-} // namespace gtsam
+}  // namespace gtsam

gtsam/nonlinear/tests/testCallRecord.cpp

@@ -153,7 +153,7 @@ TEST(CallRecord, virtualReverseAdDispatching) {
   }
   {
     const int Rows = 6;
-    record.CallRecord::reverseAD2(Eigen::Matrix<double, Rows, Cols>(), NJM);
+    record.CallRecord::reverseAD2(Eigen::Matrix<double, Rows, Cols>::Zero(), NJM);
     EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols))));
     record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM);
     EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols))));
@@ -168,4 +168,3 @@ int main() {
   return TestRegistry::runAllTests(tr);
 }
 /* ************************************************************************* */
-

gtsam/sam/tests/testBearingFactor.cpp

@@ -21,14 +21,13 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/nonlinear/factorTesting.h>
 #include <gtsam/nonlinear/expressionTesting.h>
-#include <gtsam/base/serialization.h>
-#include <gtsam/base/serializationTestHelpers.h>
 
 #include <CppUnitLite/TestHarness.h>
 
 using namespace std;
 using namespace gtsam;
 
+namespace {
 Key poseKey(1);
 Key pointKey(2);
 
@@ -41,43 +40,18 @@ typedef BearingFactor<Pose3, Point3> BearingFactor3D;
 Unit3 measurement3D = Pose3().bearing(Point3(1, 0, 0));  // has to match values!
 static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(2, 0.5));
 BearingFactor3D factor3D(poseKey, pointKey, measurement3D, model3D);
-
-/* ************************************************************************* */
-// Export Noisemodels
-// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
-BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic);
-
-/* ************************************************************************* */
-TEST(BearingFactor, Serialization2D) {
-  EXPECT(serializationTestHelpers::equalsObj(factor2D));
-  EXPECT(serializationTestHelpers::equalsXML(factor2D));
-  EXPECT(serializationTestHelpers::equalsBinary(factor2D));
 }
 
 /* ************************************************************************* */
 TEST(BearingFactor, 2D) {
-  // Serialize the factor
-  std::string serialized = serializeXML(factor2D);
-
-  // And de-serialize it
-  BearingFactor2D factor;
-  deserializeXML(serialized, factor);
-
   // Set the linearization point
   Values values;
   values.insert(poseKey, Pose2(1.0, 2.0, 0.57));
   values.insert(pointKey, Point2(-4.0, 11.0));
 
-  EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression({poseKey, pointKey}),
+  EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor2D.expression({poseKey, pointKey}),
                                       values, 1e-7, 1e-5);
-  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor2D, values, 1e-7, 1e-5);
-}
-
-/* ************************************************************************* */
-TEST(BearingFactor, Serialization3D) {
-  EXPECT(serializationTestHelpers::equalsObj(factor3D));
-  EXPECT(serializationTestHelpers::equalsXML(factor3D));
-  EXPECT(serializationTestHelpers::equalsBinary(factor3D));
 }
 
 /* ************************************************************************* */

gtsam/sam/tests/testBearingRangeFactor.cpp

@@ -21,14 +21,13 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/nonlinear/factorTesting.h>
 #include <gtsam/nonlinear/expressionTesting.h>
-#include <gtsam/base/serialization.h>
-#include <gtsam/base/serializationTestHelpers.h>
 
 #include <CppUnitLite/TestHarness.h>
 
 using namespace std;
 using namespace gtsam;
 
+namespace {
 Key poseKey(1);
 Key pointKey(2);
 
@@ -40,43 +39,18 @@ typedef BearingRangeFactor<Pose3, Point3> BearingRangeFactor3D;
 static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(3, 0.5));
 BearingRangeFactor3D factor3D(poseKey, pointKey,
                               Pose3().bearing(Point3(1, 0, 0)), 1, model3D);
-
-/* ************************************************************************* */
-// Export Noisemodels
-// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
-BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic);
-
-/* ************************************************************************* */
-TEST(BearingRangeFactor, Serialization2D) {
-  EXPECT(serializationTestHelpers::equalsObj(factor2D));
-  EXPECT(serializationTestHelpers::equalsXML(factor2D));
-  EXPECT(serializationTestHelpers::equalsBinary(factor2D));
 }
 
 /* ************************************************************************* */
 TEST(BearingRangeFactor, 2D) {
-  // Serialize the factor
-  std::string serialized = serializeXML(factor2D);
-
-  // And de-serialize it
-  BearingRangeFactor2D factor;
-  deserializeXML(serialized, factor);
-
   // Set the linearization point
   Values values;
   values.insert(poseKey, Pose2(1.0, 2.0, 0.57));
   values.insert(pointKey, Point2(-4.0, 11.0));
 
-  EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression({poseKey, pointKey}),
+  EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor2D.expression({poseKey, pointKey}),
                                       values, 1e-7, 1e-5);
-  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor2D, values, 1e-7, 1e-5);
-}
-
-/* ************************************************************************* */
-TEST(BearingRangeFactor, Serialization3D) {
-  EXPECT(serializationTestHelpers::equalsObj(factor3D));
-  EXPECT(serializationTestHelpers::equalsXML(factor3D));
-  EXPECT(serializationTestHelpers::equalsBinary(factor3D));
 }
 
 /* ************************************************************************* */

gtsam/sam/tests/testRangeFactor.cpp

@@ -22,7 +22,6 @@
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/base/numericalDerivative.h>
-#include <gtsam/base/serializationTestHelpers.h>
 #include <gtsam/base/TestableAssertions.h>
 
 #include <CppUnitLite/TestHarness.h>
@@ -32,42 +31,40 @@ using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
-// Create a noise model for the pixel error
-static SharedNoiseModel model(noiseModel::Unit::Create(1));
-
 typedef RangeFactor<Pose2, Point2> RangeFactor2D;
 typedef RangeFactor<Pose3, Point3> RangeFactor3D;
 typedef RangeFactorWithTransform<Pose2, Point2> RangeFactorWithTransform2D;
 typedef RangeFactorWithTransform<Pose3, Point3> RangeFactorWithTransform3D;
 
 // Keys are deliberately *not* in sorted order to test that case.
+namespace {
+// Create a noise model for the pixel error
+static SharedNoiseModel model(noiseModel::Unit::Create(1));
+
 constexpr Key poseKey(2);
 constexpr Key pointKey(1);
 constexpr double measurement(10.0);
 
-/* ************************************************************************* */
 Vector factorError2D(const Pose2& pose, const Point2& point,
-    const RangeFactor2D& factor) {
+                     const RangeFactor2D& factor) {
   return factor.evaluateError(pose, point);
 }
 
-/* ************************************************************************* */
 Vector factorError3D(const Pose3& pose, const Point3& point,
-    const RangeFactor3D& factor) {
+                     const RangeFactor3D& factor) {
   return factor.evaluateError(pose, point);
 }
 
-/* ************************************************************************* */
 Vector factorErrorWithTransform2D(const Pose2& pose, const Point2& point,
-    const RangeFactorWithTransform2D& factor) {
+                                  const RangeFactorWithTransform2D& factor) {
   return factor.evaluateError(pose, point);
 }
 
-/* ************************************************************************* */
 Vector factorErrorWithTransform3D(const Pose3& pose, const Point3& point,
-    const RangeFactorWithTransform3D& factor) {
+                                  const RangeFactorWithTransform3D& factor) {
   return factor.evaluateError(pose, point);
 }
+}  // namespace
 
 /* ************************************************************************* */
 TEST( RangeFactor, Constructor) {
@@ -75,27 +72,6 @@ TEST( RangeFactor, Constructor) {
   RangeFactor3D factor3D(poseKey, pointKey, measurement, model);
 }
 
-/* ************************************************************************* */
-// Export Noisemodels
-// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
-BOOST_CLASS_EXPORT(gtsam::noiseModel::Unit);
-
-/* ************************************************************************* */
-TEST(RangeFactor, Serialization2D) {
-  RangeFactor2D factor2D(poseKey, pointKey, measurement, model);
-  EXPECT(serializationTestHelpers::equalsObj(factor2D));
-  EXPECT(serializationTestHelpers::equalsXML(factor2D));
-  EXPECT(serializationTestHelpers::equalsBinary(factor2D));
-}
-
-/* ************************************************************************* */
-TEST(RangeFactor, Serialization3D) {
-  RangeFactor3D factor3D(poseKey, pointKey, measurement, model);
-  EXPECT(serializationTestHelpers::equalsObj(factor3D));
-  EXPECT(serializationTestHelpers::equalsXML(factor3D));
-  EXPECT(serializationTestHelpers::equalsBinary(factor3D));
-}
-
 /* ************************************************************************* */
 TEST( RangeFactor, ConstructorWithTransform) {
   Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2);
@@ -142,28 +118,6 @@ TEST( RangeFactor, EqualsWithTransform ) {
       body_P_sensor_3D);
   CHECK(assert_equal(factor3D_1, factor3D_2));
 }
-/* ************************************************************************* */
-TEST( RangeFactor, EqualsAfterDeserializing) {
-  // Check that the same results are obtained after deserializing:
-  Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
-      Point3(0.25, -0.10, 1.0));
-
-  RangeFactorWithTransform3D factor3D_1(poseKey, pointKey, measurement, model,
-      body_P_sensor_3D), factor3D_2;
-
-  // check with Equal() trait:
-  gtsam::serializationTestHelpers::roundtripXML(factor3D_1, factor3D_2);
-  CHECK(assert_equal(factor3D_1, factor3D_2));
-
-  const Pose3 pose(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0, -3.0));
-  const Point3 point(-2.0, 11.0, 1.0);
-  const Values values = {{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}};
-  
-  const Vector error_1 = factor3D_1.unwhitenedError(values);
-  const Vector error_2 = factor3D_2.unwhitenedError(values);
-  CHECK(assert_equal(error_1, error_2));
-}
-
 /* ************************************************************************* */
 TEST( RangeFactor, Error2D ) {
   // Create a factor
@@ -411,7 +365,7 @@ TEST( RangeFactor, Camera) {
 /* ************************************************************************* */
 // Do a test with non GTSAM types
 
-namespace gtsam{
+namespace gtsam {
 template <>
 struct Range<Vector4, Vector4> {
   typedef double result_type;
@@ -421,7 +375,7 @@ struct Range<Vector4, Vector4> {
     // derivatives not implemented
   }
 };
-}
+}  // namespace gtsam
 
 TEST(RangeFactor, NonGTSAM) {
   // Create a factor

gtsam/sam/tests/testSerializationSam.cpp

@@ -0,0 +1,140 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file  testSerializationSam.cpp
+ *  @brief All serialization test in this directory
+ *  @author Frank Dellaert
+ *  @date February 2022
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/serialization.h>
+#include <gtsam/base/serializationTestHelpers.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/nonlinear/expressionTesting.h>
+#include <gtsam/nonlinear/factorTesting.h>
+#include <gtsam/sam/BearingFactor.h>
+#include <gtsam/sam/BearingRangeFactor.h>
+#include <gtsam/sam/RangeFactor.h>
+
+using namespace std;
+using namespace gtsam;
+
+namespace {
+Key poseKey(1);
+Key pointKey(2);
+constexpr double rangeMmeasurement(10.0);
+}  // namespace
+
+/* ************************************************************************* */
+// Export Noisemodels
+// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
+BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic);
+BOOST_CLASS_EXPORT(gtsam::noiseModel::Unit);
+
+/* ************************************************************************* */
+TEST(SerializationSam, BearingFactor2D) {
+  using BearingFactor2D = BearingFactor<Pose2, Point2>;
+  double measurement2D(10.0);
+  static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(1, 0.5));
+  BearingFactor2D factor2D(poseKey, pointKey, measurement2D, model2D);
+  EXPECT(serializationTestHelpers::equalsObj(factor2D));
+  EXPECT(serializationTestHelpers::equalsXML(factor2D));
+  EXPECT(serializationTestHelpers::equalsBinary(factor2D));
+}
+
+/* ************************************************************************* */
+TEST(SerializationSam, BearingFactor3D) {
+  using BearingFactor3D = BearingFactor<Pose3, Point3>;
+  Unit3 measurement3D =
+      Pose3().bearing(Point3(1, 0, 0));  // has to match values!
+  static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(2, 0.5));
+  BearingFactor3D factor3D(poseKey, pointKey, measurement3D, model3D);
+  EXPECT(serializationTestHelpers::equalsObj(factor3D));
+  EXPECT(serializationTestHelpers::equalsXML(factor3D));
+  EXPECT(serializationTestHelpers::equalsBinary(factor3D));
+}
+
+/* ************************************************************************* */
+namespace {
+static SharedNoiseModel rangeNoiseModel(noiseModel::Unit::Create(1));
+}
+
+TEST(SerializationSam, RangeFactor2D) {
+  using RangeFactor2D = RangeFactor<Pose2, Point2>;
+  RangeFactor2D factor2D(poseKey, pointKey, rangeMmeasurement, rangeNoiseModel);
+  EXPECT(serializationTestHelpers::equalsObj(factor2D));
+  EXPECT(serializationTestHelpers::equalsXML(factor2D));
+  EXPECT(serializationTestHelpers::equalsBinary(factor2D));
+}
+
+/* ************************************************************************* */
+TEST(SerializationSam, RangeFactor3D) {
+  using RangeFactor3D = RangeFactor<Pose3, Point3>;
+  RangeFactor3D factor3D(poseKey, pointKey, rangeMmeasurement, rangeNoiseModel);
+  EXPECT(serializationTestHelpers::equalsObj(factor3D));
+  EXPECT(serializationTestHelpers::equalsXML(factor3D));
+  EXPECT(serializationTestHelpers::equalsBinary(factor3D));
+}
+
+/* ************************************************************************* */
+TEST(RangeFactor, EqualsAfterDeserializing) {
+  // Check that the same results are obtained after deserializing:
+  Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
+                         Point3(0.25, -0.10, 1.0));
+  RangeFactorWithTransform<Pose3, Point3> factor3D_1(
+      poseKey, pointKey, rangeMmeasurement, rangeNoiseModel, body_P_sensor_3D),
+      factor3D_2;
+
+  // check with Equal() trait:
+  gtsam::serializationTestHelpers::roundtripXML(factor3D_1, factor3D_2);
+  CHECK(assert_equal(factor3D_1, factor3D_2));
+
+  const Pose3 pose(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0, -3.0));
+  const Point3 point(-2.0, 11.0, 1.0);
+  const Values values = {{poseKey, genericValue(pose)},
+                         {pointKey, genericValue(point)}};
+
+  const Vector error_1 = factor3D_1.unwhitenedError(values);
+  const Vector error_2 = factor3D_2.unwhitenedError(values);
+  CHECK(assert_equal(error_1, error_2));
+}
+
+/* ************************************************************************* */
+TEST(BearingRangeFactor, Serialization2D) {
+  using BearingRangeFactor2D = BearingRangeFactor<Pose2, Point2>;
+  static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(2, 0.5));
+  BearingRangeFactor2D factor2D(poseKey, pointKey, 1, 2, model2D);
+
+  EXPECT(serializationTestHelpers::equalsObj(factor2D));
+  EXPECT(serializationTestHelpers::equalsXML(factor2D));
+  EXPECT(serializationTestHelpers::equalsBinary(factor2D));
+}
+
+/* ************************************************************************* */
+TEST(BearingRangeFactor, Serialization3D) {
+  using BearingRangeFactor3D = BearingRangeFactor<Pose3, Point3>;
+  static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(3, 0.5));
+  BearingRangeFactor3D factor3D(poseKey, pointKey,
+                                Pose3().bearing(Point3(1, 0, 0)), 1, model3D);
+  EXPECT(serializationTestHelpers::equalsObj(factor3D));
+  EXPECT(serializationTestHelpers::equalsXML(factor3D));
+  EXPECT(serializationTestHelpers::equalsBinary(factor3D));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

gtsam/slam/tests/PinholeFactor.h

@@ -0,0 +1,52 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file   PinholeFactor.h
+ *  @brief  helper class for tests
+ *  @author Frank Dellaert
+ *  @date   February 2022
+ */
+
+#pragma once
+
+namespace gtsam {
+template <typename T>
+struct traits;
+}
+
+#include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/slam/SmartFactorBase.h>
+
+namespace gtsam {
+
+class PinholeFactor : public SmartFactorBase<PinholeCamera<Cal3Bundler> > {
+ public:
+  typedef SmartFactorBase<PinholeCamera<Cal3Bundler> > Base;
+  PinholeFactor() {}
+  PinholeFactor(const SharedNoiseModel& sharedNoiseModel,
+                boost::optional<Pose3> body_P_sensor = boost::none,
+                size_t expectedNumberCameras = 10)
+      : Base(sharedNoiseModel, body_P_sensor, expectedNumberCameras) {}
+  double error(const Values& values) const override { return 0.0; }
+  boost::shared_ptr<GaussianFactor> linearize(
+      const Values& values) const override {
+    return boost::shared_ptr<GaussianFactor>(new JacobianFactor());
+  }
+};
+
+/// traits
+template <>
+struct traits<PinholeFactor> : public Testable<PinholeFactor> {};
+
+}  // namespace gtsam

gtsam/slam/tests/smartFactorScenarios.h

@@ -29,6 +29,7 @@
 using namespace std;
 using namespace gtsam;
 
+namespace {
 // three landmarks ~5 meters infront of camera
 Point3 landmark1(5, 0.5, 1.2);
 Point3 landmark2(5, -0.5, 1.2);
@@ -48,23 +49,24 @@ static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
 
 static double fov = 60;  // degrees
 static size_t w = 640, h = 480;
+}
 
 /* ************************************************************************* */
 // default Cal3_S2 cameras
 namespace vanilla {
 typedef PinholeCamera<Cal3_S2> Camera;
 typedef SmartProjectionFactor<Camera> SmartFactor;
-static Cal3_S2 K(fov, w, h);
+static const Cal3_S2 K(fov, w, h);
-static Cal3_S2 K2(1500, 1200, 0, w, h);
+static const Cal3_S2 K2(1500, 1200, 0, w, h);
-Camera level_camera(level_pose, K2);
+static const Camera level_camera(level_pose, K2);
-Camera level_camera_right(pose_right, K2);
+static const Camera level_camera_right(pose_right, K2);
-Point2 level_uv = level_camera.project(landmark1);
+static const Point2 level_uv = level_camera.project(landmark1);
-Point2 level_uv_right = level_camera_right.project(landmark1);
+static const Point2 level_uv_right = level_camera_right.project(landmark1);
-Camera cam1(level_pose, K2);
+static const Camera cam1(level_pose, K2);
-Camera cam2(pose_right, K2);
+static const Camera cam2(pose_right, K2);
-Camera cam3(pose_above, K2);
+static const Camera cam3(pose_above, K2);
 typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
-SmartProjectionParams params;
+static const SmartProjectionParams params;
 }  // namespace vanilla
 
 /* ************************************************************************* */
@@ -74,12 +76,12 @@ typedef PinholePose<Cal3_S2> Camera;
 typedef CameraSet<Camera> Cameras;
 typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
 typedef SmartProjectionRigFactor<Camera> SmartRigFactor;
-static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h));
+static const Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h));
-Camera level_camera(level_pose, sharedK);
+static const Camera level_camera(level_pose, sharedK);
-Camera level_camera_right(pose_right, sharedK);
+static const Camera level_camera_right(pose_right, sharedK);
-Camera cam1(level_pose, sharedK);
+static const Camera cam1(level_pose, sharedK);
-Camera cam2(pose_right, sharedK);
+static const Camera cam2(pose_right, sharedK);
-Camera cam3(pose_above, sharedK);
+static const Camera cam3(pose_above, sharedK);
 }  // namespace vanillaPose
 
 /* ************************************************************************* */
@@ -89,12 +91,12 @@ typedef PinholePose<Cal3_S2> Camera;
 typedef CameraSet<Camera> Cameras;
 typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
 typedef SmartProjectionRigFactor<Camera> SmartRigFactor;
-static Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480));
+static const Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480));
-Camera level_camera(level_pose, sharedK2);
+static const Camera level_camera(level_pose, sharedK2);
-Camera level_camera_right(pose_right, sharedK2);
+static const Camera level_camera_right(pose_right, sharedK2);
-Camera cam1(level_pose, sharedK2);
+static const Camera cam1(level_pose, sharedK2);
-Camera cam2(pose_right, sharedK2);
+static const Camera cam2(pose_right, sharedK2);
-Camera cam3(pose_above, sharedK2);
+static const Camera cam3(pose_above, sharedK2);
 }  // namespace vanillaPose2
 
 /* *************************************************************************/
@@ -103,15 +105,15 @@ namespace bundler {
 typedef PinholeCamera<Cal3Bundler> Camera;
 typedef CameraSet<Camera> Cameras;
 typedef SmartProjectionFactor<Camera> SmartFactor;
-static Cal3Bundler K(500, 1e-3, 1e-3, 0, 0);
+static const Cal3Bundler K(500, 1e-3, 1e-3, 0, 0);
-Camera level_camera(level_pose, K);
+static const Camera level_camera(level_pose, K);
-Camera level_camera_right(pose_right, K);
+static const Camera level_camera_right(pose_right, K);
-Point2 level_uv = level_camera.project(landmark1);
+static const Point2 level_uv = level_camera.project(landmark1);
-Point2 level_uv_right = level_camera_right.project(landmark1);
+static const Point2 level_uv_right = level_camera_right.project(landmark1);
-Pose3 pose1 = level_pose;
+static const Pose3 pose1 = level_pose;
-Camera cam1(level_pose, K);
+static const Camera cam1(level_pose, K);
-Camera cam2(pose_right, K);
+static const Camera cam2(pose_right, K);
-Camera cam3(pose_above, K);
+static const Camera cam3(pose_above, K);
 typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
 }  // namespace bundler
 
@@ -122,14 +124,14 @@ typedef PinholePose<Cal3Bundler> Camera;
 typedef CameraSet<Camera> Cameras;
 typedef SmartProjectionPoseFactor<Cal3Bundler> SmartFactor;
 typedef SmartProjectionRigFactor<Camera> SmartRigFactor;
-static boost::shared_ptr<Cal3Bundler> sharedBundlerK(new Cal3Bundler(500, 1e-3,
+static const boost::shared_ptr<Cal3Bundler> sharedBundlerK(new Cal3Bundler(500, 1e-3,
                                                                      1e-3, 1000,
                                                                      2000));
-Camera level_camera(level_pose, sharedBundlerK);
+static const Camera level_camera(level_pose, sharedBundlerK);
-Camera level_camera_right(pose_right, sharedBundlerK);
+static const Camera level_camera_right(pose_right, sharedBundlerK);
-Camera cam1(level_pose, sharedBundlerK);
+static const Camera cam1(level_pose, sharedBundlerK);
-Camera cam2(pose_right, sharedBundlerK);
+static const Camera cam2(pose_right, sharedBundlerK);
-Camera cam3(pose_above, sharedBundlerK);
+static const Camera cam3(pose_above, sharedBundlerK);
 }  // namespace bundlerPose
 
 /* ************************************************************************* */
@@ -138,12 +140,12 @@ namespace sphericalCamera {
 typedef SphericalCamera Camera;
 typedef CameraSet<Camera> Cameras;
 typedef SmartProjectionRigFactor<Camera> SmartFactorP;
-static EmptyCal::shared_ptr emptyK(new EmptyCal());
+static const EmptyCal::shared_ptr emptyK(new EmptyCal());
-Camera level_camera(level_pose);
+static const Camera level_camera(level_pose);
-Camera level_camera_right(pose_right);
+static const Camera level_camera_right(pose_right);
-Camera cam1(level_pose);
+static const Camera cam1(level_pose);
-Camera cam2(pose_right);
+static const Camera cam2(pose_right);
-Camera cam3(pose_above);
+static const Camera cam3(pose_above);
 }  // namespace sphericalCamera
 /* *************************************************************************/
 

gtsam/slam/tests/testSerializationInSlam.cpp

@@ -0,0 +1,105 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file  testSerializationSlam.cpp
+ *  @brief all serialization tests in this directory
+ *  @author Frank Dellaert
+ *  @date   February 2022
+ */
+
+#include "smartFactorScenarios.h"
+#include "PinholeFactor.h"
+
+#include <gtsam/slam/SmartProjectionFactor.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/base/serializationTestHelpers.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <boost/assign/std/map.hpp>
+#include <iostream>
+
+namespace {
+static const double rankTol = 1.0;
+static const double sigma = 0.1;
+static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma));
+}  // namespace
+
+/* ************************************************************************* */
+BOOST_CLASS_EXPORT_GUID(noiseModel::Constrained, "gtsam_noiseModel_Constrained")
+BOOST_CLASS_EXPORT_GUID(noiseModel::Diagonal, "gtsam_noiseModel_Diagonal")
+BOOST_CLASS_EXPORT_GUID(noiseModel::Gaussian, "gtsam_noiseModel_Gaussian")
+BOOST_CLASS_EXPORT_GUID(noiseModel::Unit, "gtsam_noiseModel_Unit")
+BOOST_CLASS_EXPORT_GUID(noiseModel::Isotropic, "gtsam_noiseModel_Isotropic")
+BOOST_CLASS_EXPORT_GUID(SharedNoiseModel, "gtsam_SharedNoiseModel")
+BOOST_CLASS_EXPORT_GUID(SharedDiagonal, "gtsam_SharedDiagonal")
+
+/* ************************************************************************* */
+TEST(SmartFactorBase, serialize) {
+  using namespace serializationTestHelpers;
+  PinholeFactor factor(unit2);
+
+  EXPECT(equalsObj(factor));
+  EXPECT(equalsXML(factor));
+  EXPECT(equalsBinary(factor));
+}
+
+/* ************************************************************************* */
+TEST(SerializationSlam, SmartProjectionFactor) {
+  using namespace vanilla;
+  using namespace serializationTestHelpers;
+  SmartFactor factor(unit2);
+
+  EXPECT(equalsObj(factor));
+  EXPECT(equalsXML(factor));
+  EXPECT(equalsBinary(factor));
+}
+
+/* ************************************************************************* */
+TEST(SerializationSlam, SmartProjectionPoseFactor) {
+  using namespace vanillaPose;
+  using namespace serializationTestHelpers;
+  SmartProjectionParams params;
+  params.setRankTolerance(rankTol);
+  SmartFactor factor(model, sharedK, params);
+
+  EXPECT(equalsObj(factor));
+  EXPECT(equalsXML(factor));
+  EXPECT(equalsBinary(factor));
+}
+
+TEST(SerializationSlam, SmartProjectionPoseFactor2) {
+  using namespace vanillaPose;
+  using namespace serializationTestHelpers;
+  SmartProjectionParams params;
+  params.setRankTolerance(rankTol);
+  Pose3 bts;
+  SmartFactor factor(model, sharedK, bts, params);
+
+  // insert some measurments
+  KeyVector key_view;
+  Point2Vector meas_view;
+  key_view.push_back(Symbol('x', 1));
+  meas_view.push_back(Point2(10, 10));
+  factor.add(meas_view, key_view);
+
+  EXPECT(equalsObj(factor));
+  EXPECT(equalsXML(factor));
+  EXPECT(equalsBinary(factor));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

gtsam/slam/tests/testSmartFactorBase.cpp

@@ -16,47 +16,29 @@
  *  @date   Feb 2015
  */
 
-#include <gtsam/slam/SmartFactorBase.h>
-#include <gtsam/geometry/Pose3.h>
-#include <gtsam/base/serializationTestHelpers.h>
 #include <CppUnitLite/TestHarness.h>
+#include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/StereoCamera.h>
+#include <gtsam/slam/SmartFactorBase.h>
 
-using namespace std;
+#include "PinholeFactor.h"
+
+namespace {
 using namespace gtsam;
-
 static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
 static SharedNoiseModel unit3(noiseModel::Unit::Create(3));
+}  // namespace
 
-/* ************************************************************************* */
+using namespace std;
-#include <gtsam/geometry/PinholeCamera.h>
-#include <gtsam/geometry/Cal3Bundler.h>
 
 namespace gtsam {
 
-class PinholeFactor: public SmartFactorBase<PinholeCamera<Cal3Bundler> > {
-public:
-  typedef SmartFactorBase<PinholeCamera<Cal3Bundler> > Base;
-  PinholeFactor() {}
-  PinholeFactor(const SharedNoiseModel& sharedNoiseModel,
-                boost::optional<Pose3> body_P_sensor = boost::none,
-                size_t expectedNumberCameras = 10)
-      : Base(sharedNoiseModel, body_P_sensor, expectedNumberCameras) {}
-  double error(const Values& values) const override { return 0.0; }
-  boost::shared_ptr<GaussianFactor> linearize(
-      const Values& values) const override {
-    return boost::shared_ptr<GaussianFactor>(new JacobianFactor());
-  }
-};
-
-/// traits
-template<>
-struct traits<PinholeFactor> : public Testable<PinholeFactor> {};
-}
-
 TEST(SmartFactorBase, Pinhole) {
-  PinholeFactor f= PinholeFactor(unit2);
+  PinholeFactor f = PinholeFactor(unit2);
-  f.add(Point2(0,0), 1);
+  f.add(Point2(0, 0), 1);
-  f.add(Point2(0,0), 2);
+  f.add(Point2(0, 0), 2);
   EXPECT_LONGS_EQUAL(2 * 2, f.dim());
 }
 
@@ -71,7 +53,7 @@ TEST(SmartFactorBase, PinholeWithSensor) {
   // Camera coordinates in world frame.
   Pose3 wTc = world_P_body * body_P_sensor;
   cameras.push_back(PinholeCamera<Cal3Bundler>(wTc));
-  
+
   // Simple point to project slightly off image center
   Point3 p(0, 0, 10);
   Point2 measurement = cameras[0].project(p);
@@ -81,9 +63,10 @@ TEST(SmartFactorBase, PinholeWithSensor) {
   Matrix E;
   Vector error = f.unwhitenedError<Point3>(cameras, p, Fs, E);
 
-  Vector expectedError = Vector::Zero(2);  
+  Vector expectedError = Vector::Zero(2);
   Matrix29 expectedFs;
-  expectedFs << -0.001, -1.00001, 0, -0.1, 0, -0.01, 0, 0, 0, 1, 0, 0, 0, -0.1, 0, 0, 0, 0;
+  expectedFs << -0.001, -1.00001, 0, -0.1, 0, -0.01, 0, 0, 0, 1, 0, 0, 0, -0.1,
+      0, 0, 0, 0;
   Matrix23 expectedE;
   expectedE << 0.1, 0, 0.01, 0, 0.1, 0;
 
@@ -94,20 +77,13 @@ TEST(SmartFactorBase, PinholeWithSensor) {
   EXPECT(assert_equal(expectedE, E));
 }
 
-/* ************************************************************************* */
+class StereoFactor : public SmartFactorBase<StereoCamera> {
-#include <gtsam/geometry/StereoCamera.h>
+ public:
-
-namespace gtsam {
-
-class StereoFactor: public SmartFactorBase<StereoCamera> {
-public:
   typedef SmartFactorBase<StereoCamera> Base;
   StereoFactor() {}
-  StereoFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) {
+  StereoFactor(const SharedNoiseModel& sharedNoiseModel)
-  }
+      : Base(sharedNoiseModel) {}
-  double error(const Values& values) const override {
+  double error(const Values& values) const override { return 0.0; }
-    return 0.0;
-  }
   boost::shared_ptr<GaussianFactor> linearize(
       const Values& values) const override {
     return boost::shared_ptr<GaussianFactor>(new JacobianFactor());
@@ -115,9 +91,8 @@ public:
 };
 
 /// traits
-template<>
+template <>
 struct traits<StereoFactor> : public Testable<StereoFactor> {};
-}
 
 TEST(SmartFactorBase, Stereo) {
   StereoFactor f(unit3);
@@ -125,24 +100,7 @@ TEST(SmartFactorBase, Stereo) {
   f.add(StereoPoint2(), 2);
   EXPECT_LONGS_EQUAL(2 * 3, f.dim());
 }
-
+}  // namespace gtsam
-/* ************************************************************************* */
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic")
-BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel")
-BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal")
-
-TEST(SmartFactorBase, serialize) {
-  using namespace gtsam::serializationTestHelpers;
-  PinholeFactor factor(unit2);
-
-  EXPECT(equalsObj(factor));
-  EXPECT(equalsXML(factor));
-  EXPECT(equalsBinary(factor));
-}
 
 /* ************************************************************************* */
 int main() {
@@ -150,4 +108,3 @@ int main() {
   return TestRegistry::runAllTests(tr);
 }
 /* ************************************************************************* */
-

gtsam/slam/tests/testSmartProjectionFactor.cpp

@@ -22,18 +22,19 @@
 #include "smartFactorScenarios.h"
 #include <gtsam/slam/SmartProjectionFactor.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/base/serializationTestHelpers.h>
 #include <CppUnitLite/TestHarness.h>
 #include <boost/assign/std/map.hpp>
 #include <iostream>
 
 using namespace boost::assign;
 
+namespace {
 static const bool isDebugTest = false;
 static const Symbol l1('l', 1), l2('l', 2), l3('l', 3);
 static const Key c1 = 1, c2 = 2, c3 = 3;
 static const Point2 measurement1(323.0, 240.0);
 static const double rankTol = 1.0;
+}
 
 template<class CALIBRATION>
 PinholeCamera<CALIBRATION> perturbCameraPoseAndCalibration(
@@ -70,8 +71,9 @@ TEST(SmartProjectionFactor, Constructor) {
 /* ************************************************************************* */
 TEST(SmartProjectionFactor, Constructor2) {
   using namespace vanilla;
-  params.setRankTolerance(rankTol);
+  auto myParams = params;
-  SmartFactor factor1(unit2, params);
+  myParams.setRankTolerance(rankTol);
+  SmartFactor factor1(unit2, myParams);
 }
 
 /* ************************************************************************* */
@@ -84,8 +86,9 @@ TEST(SmartProjectionFactor, Constructor3) {
 /* ************************************************************************* */
 TEST(SmartProjectionFactor, Constructor4) {
   using namespace vanilla;
-  params.setRankTolerance(rankTol);
+  auto myParams = params;
-  SmartFactor factor1(unit2, params);
+  myParams.setRankTolerance(rankTol);
+  SmartFactor factor1(unit2, myParams);
   factor1.add(measurement1, c1);
 }
 
@@ -810,25 +813,6 @@ TEST(SmartProjectionFactor, implicitJacobianFactor ) {
   EXPECT(assert_equal(yActual, yExpected, 1e-7));
 }
 
-/* ************************************************************************* */
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic")
-BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel")
-BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal")
-
-TEST(SmartProjectionFactor, serialize) {
-  using namespace vanilla;
-  using namespace gtsam::serializationTestHelpers;
-  SmartFactor factor(unit2);
-
-  EXPECT(equalsObj(factor));
-  EXPECT(equalsXML(factor));
-  EXPECT(equalsBinary(factor));
-}
-
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/slam/tests/testSmartProjectionPoseFactor.cpp

@@ -24,7 +24,6 @@
 #include <gtsam/slam/PoseTranslationPrior.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/base/numericalDerivative.h>
-#include <gtsam/base/serializationTestHelpers.h>
 #include <CppUnitLite/TestHarness.h>
 #include <boost/assign/std/map.hpp>
 #include <iostream>
@@ -32,6 +31,7 @@
 using namespace boost::assign;
 using namespace std::placeholders;
 
+namespace {
 static const double rankTol = 1.0;
 // Create a noise model for the pixel error
 static const double sigma = 0.1;
@@ -51,6 +51,7 @@ static Point2 measurement1(323.0, 240.0);
 LevenbergMarquardtParams lmParams;
 // Make more verbose like so (in tests):
 // lmParams.verbosityLM = LevenbergMarquardtParams::SUMMARY;
+}
 
 /* ************************************************************************* */
 TEST( SmartProjectionPoseFactor, Constructor) {
@@ -1332,47 +1333,6 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
           values.at<Pose3>(x3)));
 }
 
-/* ************************************************************************* */
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit")
-BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic")
-BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel")
-BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal")
-
-TEST(SmartProjectionPoseFactor, serialize) {
-  using namespace vanillaPose;
-  using namespace gtsam::serializationTestHelpers;
-  SmartProjectionParams params;
-  params.setRankTolerance(rankTol);
-  SmartFactor factor(model, sharedK, params);
-
-  EXPECT(equalsObj(factor));
-  EXPECT(equalsXML(factor));
-  EXPECT(equalsBinary(factor));
-}
-
-TEST(SmartProjectionPoseFactor, serialize2) {
-  using namespace vanillaPose;
-  using namespace gtsam::serializationTestHelpers;
-  SmartProjectionParams params;
-  params.setRankTolerance(rankTol);
-  Pose3 bts;
-  SmartFactor factor(model, sharedK, bts, params);
-
-  // insert some measurments
-  KeyVector key_view;
-  Point2Vector meas_view;
-  key_view.push_back(Symbol('x', 1));
-  meas_view.push_back(Point2(10, 10));
-  factor.add(meas_view, key_view);
-
-  EXPECT(equalsObj(factor));
-  EXPECT(equalsXML(factor));
-  EXPECT(equalsBinary(factor));
-}
-
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp

@@ -31,12 +31,13 @@ using namespace std;
 using namespace boost::assign;
 using namespace gtsam;
 
+namespace {
 // make a realistic calibration matrix
 static double b = 1;
 
 static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b));
-static Cal3_S2Stereo::shared_ptr K2(
+static Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1200, 0, 640, 480,
-    new Cal3_S2Stereo(1500, 1200, 0, 640, 480, b));
+                                                      b));
 
 static SmartStereoProjectionParams params;
 
@@ -45,8 +46,8 @@ static SmartStereoProjectionParams params;
 static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1));
 
 // Convenience for named keys
-using symbol_shorthand::X;
 using symbol_shorthand::L;
+using symbol_shorthand::X;
 
 // tests data
 static Symbol x1('X', 1);
@@ -59,16 +60,19 @@ static Symbol body_P_cam3_key('P', 3);
 static Key poseKey1(x1);
 static Key poseExtrinsicKey1(body_P_cam1_key);
 static Key poseExtrinsicKey2(body_P_cam2_key);
-static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more reasonable measurement value?
+static StereoPoint2 measurement1(
-static StereoPoint2 measurement2(350.0, 200.0, 240.0); //potentially use more reasonable measurement value?
+    323.0, 300.0, 240.0);  // potentially use more reasonable measurement value?
+static StereoPoint2 measurement2(
+    350.0, 200.0, 240.0);  // potentially use more reasonable measurement value?
 static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
-    Point3(0.25, -0.10, 1.0));
+                            Point3(0.25, -0.10, 1.0));
 
 static double missing_uR = std::numeric_limits<double>::quiet_NaN();
 
 vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
-    const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) {
+                                                     const StereoCamera& cam2,
-
+                                                     const StereoCamera& cam3,
+                                                     Point3 landmark) {
   vector<StereoPoint2> measurements_cam;
 
   StereoPoint2 cam1_uv1 = cam1.project(landmark);
@@ -82,6 +86,7 @@ vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
 }
 
 LevenbergMarquardtParams lm_params;
+}  // namespace
 
 /* ************************************************************************* */
 TEST( SmartStereoProjectionFactorPP, params) {

gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp

@@ -32,13 +32,13 @@ using namespace std;
 using namespace boost::assign;
 using namespace gtsam;
 
+namespace {
 // make a realistic calibration matrix
 static double b = 1;
 
 static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b));
-static Cal3_S2Stereo::shared_ptr K2(
+static Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1200, 0, 640, 480,
-    new Cal3_S2Stereo(1500, 1200, 0, 640, 480, b));
+                                                      b));
-
 
 static SmartStereoProjectionParams params;
 
@@ -47,8 +47,8 @@ static SmartStereoProjectionParams params;
 static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1));
 
 // Convenience for named keys
-using symbol_shorthand::X;
 using symbol_shorthand::L;
+using symbol_shorthand::X;
 
 // tests data
 static Symbol x1('X', 1);
@@ -56,15 +56,17 @@ static Symbol x2('X', 2);
 static Symbol x3('X', 3);
 
 static Key poseKey1(x1);
-static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more reasonable measurement value?
+static StereoPoint2 measurement1(
+    323.0, 300.0, 240.0);  // potentially use more reasonable measurement value?
 static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
-    Point3(0.25, -0.10, 1.0));
+                            Point3(0.25, -0.10, 1.0));
 
 static double missing_uR = std::numeric_limits<double>::quiet_NaN();
 
 vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
-    const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) {
+                                                     const StereoCamera& cam2,
-
+                                                     const StereoCamera& cam3,
+                                                     Point3 landmark) {
   vector<StereoPoint2> measurements_cam;
 
   StereoPoint2 cam1_uv1 = cam1.project(landmark);
@@ -78,6 +80,7 @@ vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
 }
 
 LevenbergMarquardtParams lm_params;
+}  // namespace
 
 /* ************************************************************************* */
 TEST( SmartStereoProjectionPoseFactor, params) {

tests/testSerializationSlam.cpp

@@ -19,16 +19,16 @@
 #include <CppUnitLite/TestHarness.h>
 
 #include <tests/smallExample.h>
+
+#include <gtsam/sam/RangeFactor.h>
 #include <gtsam/sam/BearingRangeFactor.h>
+
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
-#include <gtsam/nonlinear/PriorFactor.h>
 #include <gtsam/slam/ProjectionFactor.h>
-#include <gtsam/sam/RangeFactor.h>
+#include <gtsam/slam/dataset.h>
 #include <gtsam/slam/StereoFactor.h>
-#include <gtsam/nonlinear/NonlinearEquality.h>
+
-#include <gtsam/inference/Symbol.h>
-#include <gtsam/linear/GaussianISAM.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/StereoPoint2.h>
 #include <gtsam/geometry/Point3.h>
@@ -44,8 +44,16 @@
 #include <gtsam/geometry/StereoCamera.h>
 #include <gtsam/geometry/SimpleCamera.h>
 
+#include <gtsam/nonlinear/NonlinearEquality.h>
+#include <gtsam/nonlinear/PriorFactor.h>
+#include <gtsam/linear/SubgraphPreconditioner.h>
+#include <gtsam/linear/GaussianISAM.h>
+#include <gtsam/inference/Symbol.h>
 #include <gtsam/base/serializationTestHelpers.h>
 
+#include <boost/archive/xml_iarchive.hpp>
+#include <boost/serialization/export.hpp>
+
 using namespace std;
 using namespace gtsam;
 using namespace gtsam::serializationTestHelpers;
@@ -592,6 +600,78 @@ TEST (testSerializationSLAM, factors) {
   EXPECT(equalsBinary<GenericStereoFactor3D>(genericStereoFactor3D));
 }
 
+/* ************************************************************************* */
+// Read from XML file
+namespace {
+static GaussianFactorGraph read(const string& name) {
+  auto inputFile = findExampleDataFile(name);
+  ifstream is(inputFile);
+  if (!is.is_open()) throw runtime_error("Cannot find file " + inputFile);
+  boost::archive::xml_iarchive in_archive(is);
+  GaussianFactorGraph Ab;
+  in_archive >> boost::serialization::make_nvp("graph", Ab);
+  return Ab;
+}
+}  // namespace
+
+/* ************************************************************************* */
+// Read from XML file
+TEST(SubgraphSolver, Solves) {
+  using gtsam::example::planarGraph;
+
+  // Create preconditioner
+  SubgraphPreconditioner system;
+
+  // We test on three different graphs
+  const auto Ab1 = planarGraph(3).first;
+  const auto Ab2 = read("toy3D");
+  const auto Ab3 = read("randomGrid3D");
+
+  // For all graphs, test solve and solveTranspose
+  for (const auto& Ab : {Ab1, Ab2, Ab3}) {
+    // Call build, a non-const method needed to make solve work :-(
+    KeyInfo keyInfo(Ab);
+    std::map<Key, Vector> lambda;
+    system.build(Ab, keyInfo, lambda);
+
+    // Create a perturbed (non-zero) RHS
+    const auto xbar = system.Rc1().optimize();  // merely for use in zero below
+    auto values_y = VectorValues::Zero(xbar);
+    auto it = values_y.begin();
+    it->second.setConstant(100);
+    ++it;
+    it->second.setConstant(-100);
+
+    // Solve the VectorValues way
+    auto values_x = system.Rc1().backSubstitute(values_y);
+
+    // Solve the matrix way, this really just checks BN::backSubstitute
+    // This only works with Rc1 ordering, not with keyInfo !
+    // TODO(frank): why does this not work with an arbitrary ordering?
+    const auto ord = system.Rc1().ordering();
+    const Matrix R1 = system.Rc1().matrix(ord).first;
+    auto ord_y = values_y.vector(ord);
+    auto vector_x = R1.inverse() * ord_y;
+    EXPECT(assert_equal(vector_x, values_x.vector(ord)));
+
+    // Test that 'solve' does implement x = R^{-1} y
+    // We do this by asserting it gives same answer as backSubstitute
+    // Only works with keyInfo ordering:
+    const auto ordering = keyInfo.ordering();
+    auto vector_y = values_y.vector(ordering);
+    const size_t N = R1.cols();
+    Vector solve_x = Vector::Zero(N);
+    system.solve(vector_y, solve_x);
+    EXPECT(assert_equal(values_x.vector(ordering), solve_x));
+
+    // Test that transposeSolve does implement x = R^{-T} y
+    // We do this by asserting it gives same answer as backSubstituteTranspose
+    auto values_x2 = system.Rc1().backSubstituteTranspose(values_y);
+    Vector solveT_x = Vector::Zero(N);
+    system.transposeSolve(vector_y, solveT_x);
+    EXPECT(assert_equal(values_x2.vector(ordering), solveT_x));
+  }
+}
 
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }

tests/testSubgraphPreconditioner.cpp

@@ -29,10 +29,8 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#include <boost/archive/xml_iarchive.hpp>
 #include <boost/assign/std/list.hpp>
 #include <boost/range/adaptor/reversed.hpp>
-#include <boost/serialization/export.hpp>
 #include <boost/tuple/tuple.hpp>
 using namespace boost::assign;
 
@@ -197,75 +195,6 @@ TEST(SubgraphPreconditioner, system) {
   EXPECT(assert_equal(expected_g, vec(g)));
 }
 
-/* ************************************************************************* */
-BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "JacobianFactor")
-
-// Read from XML file
-static GaussianFactorGraph read(const string& name) {
-  auto inputFile = findExampleDataFile(name);
-  ifstream is(inputFile);
-  if (!is.is_open()) throw runtime_error("Cannot find file " + inputFile);
-  boost::archive::xml_iarchive in_archive(is);
-  GaussianFactorGraph Ab;
-  in_archive >> boost::serialization::make_nvp("graph", Ab);
-  return Ab;
-}
-
-TEST(SubgraphSolver, Solves) {
-  // Create preconditioner
-  SubgraphPreconditioner system;
-
-  // We test on three different graphs
-  const auto Ab1 = planarGraph(3).first;
-  const auto Ab2 = read("toy3D");
-  const auto Ab3 = read("randomGrid3D");
-
-  // For all graphs, test solve and solveTranspose
-  for (const auto& Ab : {Ab1, Ab2, Ab3}) {
-    // Call build, a non-const method needed to make solve work :-(
-    KeyInfo keyInfo(Ab);
-    std::map<Key, Vector> lambda;
-    system.build(Ab, keyInfo, lambda);
-
-    // Create a perturbed (non-zero) RHS
-    const auto xbar = system.Rc1().optimize();  // merely for use in zero below
-    auto values_y = VectorValues::Zero(xbar);
-    auto it = values_y.begin();
-    it->second.setConstant(100);
-    ++it;
-    it->second.setConstant(-100);
-
-    // Solve the VectorValues way
-    auto values_x = system.Rc1().backSubstitute(values_y);
-
-    // Solve the matrix way, this really just checks BN::backSubstitute
-    // This only works with Rc1 ordering, not with keyInfo !
-    // TODO(frank): why does this not work with an arbitrary ordering?
-    const auto ord = system.Rc1().ordering();
-    const Matrix R1 = system.Rc1().matrix(ord).first;
-    auto ord_y = values_y.vector(ord);
-    auto vector_x = R1.inverse() * ord_y;
-    EXPECT(assert_equal(vector_x, values_x.vector(ord)));
-
-    // Test that 'solve' does implement x = R^{-1} y
-    // We do this by asserting it gives same answer as backSubstitute
-    // Only works with keyInfo ordering:
-    const auto ordering = keyInfo.ordering();
-    auto vector_y = values_y.vector(ordering);
-    const size_t N = R1.cols();
-    Vector solve_x = Vector::Zero(N);
-    system.solve(vector_y, solve_x);
-    EXPECT(assert_equal(values_x.vector(ordering), solve_x));
-
-    // Test that transposeSolve does implement x = R^{-T} y
-    // We do this by asserting it gives same answer as backSubstituteTranspose
-    auto values_x2 = system.Rc1().backSubstituteTranspose(values_y);
-    Vector solveT_x = Vector::Zero(N);
-    system.transposeSolve(vector_y, solveT_x);
-    EXPECT(assert_equal(values_x2.vector(ordering), solveT_x));
-  }
-}
-
 /* ************************************************************************* */
 TEST(SubgraphPreconditioner, conjugateGradients) {
   // Build a planar graph