diff --git a/gtsam/slam/tests/PinholeFactor.h b/gtsam/slam/tests/PinholeFactor.h new file mode 100644 index 000000000..35500ca35 --- /dev/null +++ b/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 +struct traits; +} + +#include +#include +#include +#include + +namespace gtsam { + +class PinholeFactor : public SmartFactorBase > { + public: + typedef SmartFactorBase > Base; + PinholeFactor() {} + PinholeFactor(const SharedNoiseModel& sharedNoiseModel, + boost::optional 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 linearize( + const Values& values) const override { + return boost::shared_ptr(new JacobianFactor()); + } +}; + +/// traits +template <> +struct traits : public Testable {}; + +} // namespace gtsam diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h index 66be08c67..eff942799 100644 --- a/gtsam/slam/tests/smartFactorScenarios.h +++ b/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 Camera; typedef SmartProjectionFactor SmartFactor; -static Cal3_S2 K(fov, w, h); -static Cal3_S2 K2(1500, 1200, 0, w, h); -Camera level_camera(level_pose, K2); -Camera level_camera_right(pose_right, K2); -Point2 level_uv = level_camera.project(landmark1); -Point2 level_uv_right = level_camera_right.project(landmark1); -Camera cam1(level_pose, K2); -Camera cam2(pose_right, K2); -Camera cam3(pose_above, K2); +static const Cal3_S2 K(fov, w, h); +static const Cal3_S2 K2(1500, 1200, 0, w, h); +static const Camera level_camera(level_pose, K2); +static const Camera level_camera_right(pose_right, K2); +static const Point2 level_uv = level_camera.project(landmark1); +static const Point2 level_uv_right = level_camera_right.project(landmark1); +static const Camera cam1(level_pose, K2); +static const Camera cam2(pose_right, K2); +static const Camera cam3(pose_above, K2); typedef GeneralSFMFactor SFMFactor; -SmartProjectionParams params; +static const SmartProjectionParams params; } // namespace vanilla /* ************************************************************************* */ @@ -74,12 +76,12 @@ typedef PinholePose Camera; typedef CameraSet Cameras; typedef SmartProjectionPoseFactor SmartFactor; typedef SmartProjectionRigFactor SmartRigFactor; -static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); -Camera level_camera(level_pose, sharedK); -Camera level_camera_right(pose_right, sharedK); -Camera cam1(level_pose, sharedK); -Camera cam2(pose_right, sharedK); -Camera cam3(pose_above, sharedK); +static const Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); +static const Camera level_camera(level_pose, sharedK); +static const Camera level_camera_right(pose_right, sharedK); +static const Camera cam1(level_pose, sharedK); +static const Camera cam2(pose_right, sharedK); +static const Camera cam3(pose_above, sharedK); } // namespace vanillaPose /* ************************************************************************* */ @@ -89,12 +91,12 @@ typedef PinholePose Camera; typedef CameraSet Cameras; typedef SmartProjectionPoseFactor SmartFactor; typedef SmartProjectionRigFactor SmartRigFactor; -static Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480)); -Camera level_camera(level_pose, sharedK2); -Camera level_camera_right(pose_right, sharedK2); -Camera cam1(level_pose, sharedK2); -Camera cam2(pose_right, sharedK2); -Camera cam3(pose_above, sharedK2); +static const Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480)); +static const Camera level_camera(level_pose, sharedK2); +static const Camera level_camera_right(pose_right, sharedK2); +static const Camera cam1(level_pose, sharedK2); +static const Camera cam2(pose_right, sharedK2); +static const Camera cam3(pose_above, sharedK2); } // namespace vanillaPose2 /* *************************************************************************/ @@ -103,15 +105,15 @@ namespace bundler { typedef PinholeCamera Camera; typedef CameraSet Cameras; typedef SmartProjectionFactor SmartFactor; -static Cal3Bundler K(500, 1e-3, 1e-3, 0, 0); -Camera level_camera(level_pose, K); -Camera level_camera_right(pose_right, K); -Point2 level_uv = level_camera.project(landmark1); -Point2 level_uv_right = level_camera_right.project(landmark1); -Pose3 pose1 = level_pose; -Camera cam1(level_pose, K); -Camera cam2(pose_right, K); -Camera cam3(pose_above, K); +static const Cal3Bundler K(500, 1e-3, 1e-3, 0, 0); +static const Camera level_camera(level_pose, K); +static const Camera level_camera_right(pose_right, K); +static const Point2 level_uv = level_camera.project(landmark1); +static const Point2 level_uv_right = level_camera_right.project(landmark1); +static const Pose3 pose1 = level_pose; +static const Camera cam1(level_pose, K); +static const Camera cam2(pose_right, K); +static const Camera cam3(pose_above, K); typedef GeneralSFMFactor SFMFactor; } // namespace bundler @@ -122,14 +124,14 @@ typedef PinholePose Camera; typedef CameraSet Cameras; typedef SmartProjectionPoseFactor SmartFactor; typedef SmartProjectionRigFactor SmartRigFactor; -static boost::shared_ptr sharedBundlerK(new Cal3Bundler(500, 1e-3, +static const boost::shared_ptr sharedBundlerK(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); -Camera level_camera(level_pose, sharedBundlerK); -Camera level_camera_right(pose_right, sharedBundlerK); -Camera cam1(level_pose, sharedBundlerK); -Camera cam2(pose_right, sharedBundlerK); -Camera cam3(pose_above, sharedBundlerK); +static const Camera level_camera(level_pose, sharedBundlerK); +static const Camera level_camera_right(pose_right, sharedBundlerK); +static const Camera cam1(level_pose, sharedBundlerK); +static const Camera cam2(pose_right, sharedBundlerK); +static const Camera cam3(pose_above, sharedBundlerK); } // namespace bundlerPose /* ************************************************************************* */ @@ -138,12 +140,12 @@ namespace sphericalCamera { typedef SphericalCamera Camera; typedef CameraSet Cameras; typedef SmartProjectionRigFactor SmartFactorP; -static EmptyCal::shared_ptr emptyK(new EmptyCal()); -Camera level_camera(level_pose); -Camera level_camera_right(pose_right); -Camera cam1(level_pose); -Camera cam2(pose_right); -Camera cam3(pose_above); +static const EmptyCal::shared_ptr emptyK(new EmptyCal()); +static const Camera level_camera(level_pose); +static const Camera level_camera_right(pose_right); +static const Camera cam1(level_pose); +static const Camera cam2(pose_right); +static const Camera cam3(pose_above); } // namespace sphericalCamera /* *************************************************************************/ diff --git a/gtsam/slam/tests/testSerializationSlam.cpp b/gtsam/slam/tests/testSerializationSlam.cpp new file mode 100644 index 000000000..02b0e3191 --- /dev/null +++ b/gtsam/slam/tests/testSerializationSlam.cpp @@ -0,0 +1,104 @@ +/* ---------------------------------------------------------------------------- + + * 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 +#include +#include +#include + +#include +#include + +#include "smartFactorScenarios.h" +#include "PinholeFactor.h" + +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); +} +/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp index ba46dce8d..544fd3264 100644 --- a/gtsam/slam/tests/testSmartFactorBase.cpp +++ b/gtsam/slam/tests/testSmartFactorBase.cpp @@ -16,47 +16,29 @@ * @date Feb 2015 */ -#include -#include -#include #include +#include +#include +#include +#include +#include -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 -/* ************************************************************************* */ -#include -#include +using namespace std; namespace gtsam { -class PinholeFactor: public SmartFactorBase > { -public: - typedef SmartFactorBase > Base; - PinholeFactor() {} - PinholeFactor(const SharedNoiseModel& sharedNoiseModel, - boost::optional 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 linearize( - const Values& values) const override { - return boost::shared_ptr(new JacobianFactor()); - } -}; - -/// traits -template<> -struct traits : public Testable {}; -} - TEST(SmartFactorBase, Pinhole) { - PinholeFactor f= PinholeFactor(unit2); - f.add(Point2(0,0), 1); - f.add(Point2(0,0), 2); + PinholeFactor f = PinholeFactor(unit2); + f.add(Point2(0, 0), 1); + 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(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(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)); } -/* ************************************************************************* */ -#include - -namespace gtsam { - -class StereoFactor: public SmartFactorBase { -public: +class StereoFactor : public SmartFactorBase { + public: typedef SmartFactorBase Base; StereoFactor() {} - StereoFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) { - } - double error(const Values& values) const override { - return 0.0; - } + StereoFactor(const SharedNoiseModel& sharedNoiseModel) + : Base(sharedNoiseModel) {} + double error(const Values& values) const override { return 0.0; } boost::shared_ptr linearize( const Values& values) const override { return boost::shared_ptr(new JacobianFactor()); @@ -115,9 +91,8 @@ public: }; /// traits -template<> +template <> struct traits : public Testable {}; -} TEST(SmartFactorBase, Stereo) { StereoFactor f(unit3); @@ -125,24 +100,7 @@ TEST(SmartFactorBase, Stereo) { f.add(StereoPoint2(), 2); EXPECT_LONGS_EQUAL(2 * 3, f.dim()); } - -/* ************************************************************************* */ -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)); -} +} // namespace gtsam /* ************************************************************************* */ int main() { @@ -150,4 +108,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam/slam/tests/testSmartProjectionFactor.cpp b/gtsam/slam/tests/testSmartProjectionFactor.cpp index 133f81511..ecdb5287f 100644 --- a/gtsam/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactor.cpp @@ -22,18 +22,19 @@ #include "smartFactorScenarios.h" #include #include -#include #include #include #include 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 PinholeCamera perturbCameraPoseAndCalibration( @@ -70,8 +71,9 @@ TEST(SmartProjectionFactor, Constructor) { /* ************************************************************************* */ TEST(SmartProjectionFactor, Constructor2) { using namespace vanilla; - params.setRankTolerance(rankTol); - SmartFactor factor1(unit2, params); + auto myParams = params; + myParams.setRankTolerance(rankTol); + SmartFactor factor1(unit2, myParams); } /* ************************************************************************* */ @@ -84,8 +86,9 @@ TEST(SmartProjectionFactor, Constructor3) { /* ************************************************************************* */ TEST(SmartProjectionFactor, Constructor4) { using namespace vanilla; - params.setRankTolerance(rankTol); - SmartFactor factor1(unit2, params); + auto myParams = 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; diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index f3706fa02..5c38233c1 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -24,7 +24,6 @@ #include #include #include -#include #include #include #include @@ -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(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;