From 10bf3a0199f0d630c5d08ec050c1cf27b7d8ddb5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 16 Feb 2022 08:54:36 -0500 Subject: [PATCH] fix check.sam --- gtsam/nonlinear/factorTesting.h | 39 +++--- gtsam/sam/tests/testBearingFactor.cpp | 32 +---- gtsam/sam/tests/testBearingRangeFactor.cpp | 32 +---- gtsam/sam/tests/testRangeFactor.cpp | 68 ++-------- gtsam/sam/tests/testSerializationSam.cpp | 140 +++++++++++++++++++++ 5 files changed, 179 insertions(+), 132 deletions(-) create mode 100644 gtsam/sam/tests/testSerializationSam.cpp diff --git a/gtsam/nonlinear/factorTesting.h b/gtsam/nonlinear/factorTesting.h index 3a9b6fb11..266aa841c 100644 --- a/gtsam/nonlinear/factorTesting.h +++ b/gtsam/nonlinear/factorTesting.h @@ -21,6 +21,8 @@ #include #include +#include +#include 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, - const Values& values, double delta = 1e-5) { +inline JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, + const Values& values, + double delta = 1e-5) { // We will fill a vector of key/Jacobians pairs (a map would sort) std::vector > 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_, - const NoiseModelFactor& factor, const gtsam::Values& values, double delta, - double tolerance) { - +inline bool testFactorJacobians(const std::string& name_, + const NoiseModelFactor& factor, + 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 actual = // + auto actual = boost::dynamic_pointer_cast(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)), - (Matrix) (actual->getA(actual->begin() + i)), tolerance); + bool i_good = + 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 diff --git a/gtsam/sam/tests/testBearingFactor.cpp b/gtsam/sam/tests/testBearingFactor.cpp index 17a049a1d..904bdba31 100644 --- a/gtsam/sam/tests/testBearingFactor.cpp +++ b/gtsam/sam/tests/testBearingFactor.cpp @@ -21,14 +21,13 @@ #include #include #include -#include -#include #include using namespace std; using namespace gtsam; +namespace { Key poseKey(1); Key pointKey(2); @@ -41,43 +40,18 @@ typedef BearingFactor 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); -} - -/* ************************************************************************* */ -TEST(BearingFactor, Serialization3D) { - EXPECT(serializationTestHelpers::equalsObj(factor3D)); - EXPECT(serializationTestHelpers::equalsXML(factor3D)); - EXPECT(serializationTestHelpers::equalsBinary(factor3D)); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor2D, values, 1e-7, 1e-5); } /* ************************************************************************* */ diff --git a/gtsam/sam/tests/testBearingRangeFactor.cpp b/gtsam/sam/tests/testBearingRangeFactor.cpp index 735358d89..0dcc227c7 100644 --- a/gtsam/sam/tests/testBearingRangeFactor.cpp +++ b/gtsam/sam/tests/testBearingRangeFactor.cpp @@ -21,14 +21,13 @@ #include #include #include -#include -#include #include using namespace std; using namespace gtsam; +namespace { Key poseKey(1); Key pointKey(2); @@ -40,43 +39,18 @@ typedef BearingRangeFactor 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); -} - -/* ************************************************************************* */ -TEST(BearingRangeFactor, Serialization3D) { - EXPECT(serializationTestHelpers::equalsObj(factor3D)); - EXPECT(serializationTestHelpers::equalsXML(factor3D)); - EXPECT(serializationTestHelpers::equalsBinary(factor3D)); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor2D, values, 1e-7, 1e-5); } /* ************************************************************************* */ diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp index 5f5d4f4dd..200e1236a 100644 --- a/gtsam/sam/tests/testRangeFactor.cpp +++ b/gtsam/sam/tests/testRangeFactor.cpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include @@ -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 RangeFactor2D; typedef RangeFactor RangeFactor3D; typedef RangeFactorWithTransform RangeFactorWithTransform2D; typedef RangeFactorWithTransform 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 { typedef double result_type; @@ -421,7 +375,7 @@ struct Range { // derivatives not implemented } }; -} +} // namespace gtsam TEST(RangeFactor, NonGTSAM) { // Create a factor diff --git a/gtsam/sam/tests/testSerializationSam.cpp b/gtsam/sam/tests/testSerializationSam.cpp new file mode 100644 index 000000000..8fdd8f37e --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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; + 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; + 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; + RangeFactor2D factor2D(poseKey, pointKey, rangeMmeasurement, rangeNoiseModel); + EXPECT(serializationTestHelpers::equalsObj(factor2D)); + EXPECT(serializationTestHelpers::equalsXML(factor2D)); + EXPECT(serializationTestHelpers::equalsBinary(factor2D)); +} + +/* ************************************************************************* */ +TEST(SerializationSam, RangeFactor3D) { + using RangeFactor3D = RangeFactor; + 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 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; + 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; + 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); +} +/* ************************************************************************* */