diff --git a/gtsam/slam/ReadMe.md b/gtsam/slam/ReadMe.md index c43f0769a..d216b9181 100644 --- a/gtsam/slam/ReadMe.md +++ b/gtsam/slam/ReadMe.md @@ -38,7 +38,7 @@ The factor only constrains poses. If the calibration should be optimized, as well, use `SmartProjectionFactor` instead! -### SmartProjectionFactorP +### SmartProjectionRigFactor Same as `SmartProjectionPoseFactor`, except: - it is templated on `CAMERA`, i.e., it allows cameras beyond pinhole; diff --git a/gtsam/slam/SmartProjectionFactorP.h b/gtsam/slam/SmartProjectionRigFactor.h similarity index 95% rename from gtsam/slam/SmartProjectionFactorP.h rename to gtsam/slam/SmartProjectionRigFactor.h index b01420446..26cbffe7c 100644 --- a/gtsam/slam/SmartProjectionFactorP.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file SmartProjectionFactorP.h + * @file SmartProjectionRigFactor.h * @brief Smart factor on poses, assuming camera calibration is fixed. * Same as SmartProjectionPoseFactor, except: * - it is templated on CAMERA (i.e., it allows cameras beyond pinhole) @@ -47,11 +47,11 @@ namespace gtsam { * @addtogroup SLAM */ template -class SmartProjectionFactorP : public SmartProjectionFactor { +class SmartProjectionRigFactor : public SmartProjectionFactor { private: typedef SmartProjectionFactor Base; - typedef SmartProjectionFactorP This; + typedef SmartProjectionRigFactor This; typedef typename CAMERA::CalibrationType CALIBRATION; static const int DimPose = 6; ///< Pose3 dimension @@ -76,7 +76,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { typedef boost::shared_ptr shared_ptr; /// Default constructor, only for serialization - SmartProjectionFactorP() { + SmartProjectionRigFactor() { } /** @@ -84,7 +84,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { * @param sharedNoiseModel isotropic noise model for the 2D feature measurements * @param params parameters for the smart projection factors */ - SmartProjectionFactorP(const SharedNoiseModel& sharedNoiseModel, + SmartProjectionRigFactor(const SharedNoiseModel& sharedNoiseModel, const SmartProjectionParams& params = SmartProjectionParams()) : Base(sharedNoiseModel, params) { @@ -94,7 +94,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { } /** Virtual destructor */ - ~SmartProjectionFactorP() override { + ~SmartProjectionRigFactor() override { } /** @@ -167,7 +167,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { - std::cout << s << "SmartProjectionFactorP: \n "; + std::cout << s << "SmartProjectionRigFactor: \n "; for (size_t i = 0; i < K_all_.size(); i++) { std::cout << "-- Measurement nr " << i << std::endl; std::cout << "key: " << keyFormatter(nonUniqueKeys_[i]) << std::endl; @@ -266,7 +266,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { std::vector < Vector > gs(nrUniqueKeys); if (this->measured_.size() != this->cameras(values).size()) // 1 observation per camera - throw std::runtime_error("SmartProjectionFactorP: " + throw std::runtime_error("SmartProjectionRigFactor: " "measured_.size() inconsistent with input"); // triangulate 3D point at given linearization point @@ -282,7 +282,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { > (this->keys_, Gs, gs, 0.0); } else { throw std::runtime_error( - "SmartProjectionFactorP: " + "SmartProjectionRigFactor: " "only supported degeneracy mode is ZERO_ON_DEGENERACY"); } } @@ -351,8 +351,8 @@ class SmartProjectionFactorP : public SmartProjectionFactor { /// traits template -struct traits > : public Testable< - SmartProjectionFactorP > { +struct traits > : public Testable< + SmartProjectionRigFactor > { }; } // \ namespace gtsam diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h index 7421f73af..f35bdc950 100644 --- a/gtsam/slam/tests/smartFactorScenarios.h +++ b/gtsam/slam/tests/smartFactorScenarios.h @@ -18,11 +18,11 @@ #pragma once #include -#include #include #include #include #include +#include "../SmartProjectionRigFactor.h" using namespace std; using namespace gtsam; @@ -70,7 +70,7 @@ SmartProjectionParams params; namespace vanillaPose { typedef PinholePose Camera; typedef SmartProjectionPoseFactor SmartFactor; -typedef SmartProjectionFactorP SmartFactorP; +typedef SmartProjectionRigFactor SmartFactorP; 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); @@ -84,7 +84,7 @@ Camera cam3(pose_above, sharedK); namespace vanillaPose2 { typedef PinholePose Camera; typedef SmartProjectionPoseFactor SmartFactor; -typedef SmartProjectionFactorP SmartFactorP; +typedef SmartProjectionRigFactor SmartFactorP; 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); @@ -114,7 +114,7 @@ typedef GeneralSFMFactor SFMFactor; namespace bundlerPose { typedef PinholePose Camera; typedef SmartProjectionPoseFactor SmartFactor; -typedef SmartProjectionFactorP SmartFactorP; +typedef SmartProjectionRigFactor SmartFactorP; static boost::shared_ptr sharedBundlerK( new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); Camera level_camera(level_pose, sharedBundlerK); diff --git a/gtsam/slam/tests/testSmartProjectionFactorP.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp similarity index 96% rename from gtsam/slam/tests/testSmartProjectionFactorP.cpp rename to gtsam/slam/tests/testSmartProjectionRigFactor.cpp index 4591dc08e..ef330cdd0 100644 --- a/gtsam/slam/tests/testSmartProjectionFactorP.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file testSmartProjectionFactorP.cpp - * @brief Unit tests for SmartProjectionFactorP Class + * @file testSmartProjectionRigFactor.cpp + * @brief Unit tests for SmartProjectionRigFactor Class * @author Chris Beall * @author Luca Carlone * @author Zsolt Kira @@ -52,13 +52,13 @@ LevenbergMarquardtParams lmParams; // params.verbosityLM = LevenbergMarquardtParams::SUMMARY; /* ************************************************************************* */ -TEST( SmartProjectionFactorP, Constructor) { +TEST( SmartProjectionRigFactor, Constructor) { using namespace vanillaPose; SmartFactorP::shared_ptr factor1(new SmartFactorP(model)); } /* ************************************************************************* */ -TEST( SmartProjectionFactorP, Constructor2) { +TEST( SmartProjectionRigFactor, Constructor2) { using namespace vanillaPose; SmartProjectionParams params; params.setRankTolerance(rankTol); @@ -66,14 +66,14 @@ TEST( SmartProjectionFactorP, Constructor2) { } /* ************************************************************************* */ -TEST( SmartProjectionFactorP, Constructor3) { +TEST( SmartProjectionRigFactor, Constructor3) { using namespace vanillaPose; SmartFactorP::shared_ptr factor1(new SmartFactorP(model)); factor1->add(measurement1, x1, sharedK); } /* ************************************************************************* */ -TEST( SmartProjectionFactorP, Constructor4) { +TEST( SmartProjectionRigFactor, Constructor4) { using namespace vanillaPose; SmartProjectionParams params; params.setRankTolerance(rankTol); @@ -82,7 +82,7 @@ TEST( SmartProjectionFactorP, Constructor4) { } /* ************************************************************************* */ -TEST( SmartProjectionFactorP, Equals ) { +TEST( SmartProjectionRigFactor, Equals ) { using namespace vanillaPose; SmartFactorP::shared_ptr factor1(new SmartFactorP(model)); factor1->add(measurement1, x1, sharedK); @@ -94,7 +94,7 @@ TEST( SmartProjectionFactorP, Equals ) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, noiseless ) { +TEST( SmartProjectionRigFactor, noiseless ) { using namespace vanillaPose; @@ -153,7 +153,7 @@ TEST( SmartProjectionFactorP, noiseless ) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, noisy ) { +TEST( SmartProjectionRigFactor, noisy ) { using namespace vanillaPose; @@ -191,7 +191,7 @@ TEST( SmartProjectionFactorP, noisy ) { } /* *************************************************************************/ -TEST(SmartProjectionFactorP, smartFactorWithSensorBodyTransform) { +TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { using namespace vanillaPose; // create arbitrary body_T_sensor (transforms from sensor to body) @@ -279,7 +279,7 @@ TEST(SmartProjectionFactorP, smartFactorWithSensorBodyTransform) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, 3poses_smart_projection_factor ) { +TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) { using namespace vanillaPose2; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -346,7 +346,7 @@ TEST( SmartProjectionFactorP, 3poses_smart_projection_factor ) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, Factors ) { +TEST( SmartProjectionRigFactor, Factors ) { using namespace vanillaPose; @@ -437,7 +437,7 @@ TEST( SmartProjectionFactorP, Factors ) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, 3poses_iterative_smart_projection_factor ) { +TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) { using namespace vanillaPose; @@ -497,7 +497,7 @@ TEST( SmartProjectionFactorP, 3poses_iterative_smart_projection_factor ) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, landmarkDistance ) { +TEST( SmartProjectionRigFactor, landmarkDistance ) { using namespace vanillaPose; @@ -558,7 +558,7 @@ TEST( SmartProjectionFactorP, landmarkDistance ) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, dynamicOutlierRejection ) { +TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) { using namespace vanillaPose; @@ -626,7 +626,7 @@ TEST( SmartProjectionFactorP, dynamicOutlierRejection ) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, CheckHessian) { +TEST( SmartProjectionRigFactor, CheckHessian) { KeyVector views { x1, x2, x3 }; @@ -711,7 +711,7 @@ TEST( SmartProjectionFactorP, CheckHessian) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, Hessian ) { +TEST( SmartProjectionRigFactor, Hessian ) { using namespace vanillaPose2; @@ -746,7 +746,7 @@ TEST( SmartProjectionFactorP, Hessian ) { } /* ************************************************************************* */ -TEST( SmartProjectionFactorP, ConstructorWithCal3Bundler) { +TEST( SmartProjectionRigFactor, ConstructorWithCal3Bundler) { using namespace bundlerPose; SmartProjectionParams params; params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); @@ -755,7 +755,7 @@ TEST( SmartProjectionFactorP, ConstructorWithCal3Bundler) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, Cal3Bundler ) { +TEST( SmartProjectionRigFactor, Cal3Bundler ) { using namespace bundlerPose; @@ -820,7 +820,7 @@ TEST( SmartProjectionFactorP, Cal3Bundler ) { typedef GenericProjectionFactor TestProjectionFactor; static Symbol l0('L', 0); /* *************************************************************************/ -TEST( SmartProjectionFactorP, hessianComparedToProjFactors_measurementsFromSamePose) { +TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSamePose) { // in this test we make sure the fact works even if we have multiple pixel measurements of the same landmark // at a single pose, a setup that occurs in multi-camera systems @@ -951,7 +951,7 @@ TEST( SmartProjectionFactorP, hessianComparedToProjFactors_measurementsFromSameP } /* *************************************************************************/ -TEST( SmartProjectionFactorP, optimization_3poses_measurementsFromSamePose ) { +TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) { using namespace vanillaPose; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; @@ -1033,7 +1033,7 @@ TEST( SmartProjectionFactorP, optimization_3poses_measurementsFromSamePose ) { //| -SmartFactorP LINEARIZE: 0 CPU (1000 times, 0.005481 wall, 0 children, min: 0 max: 0) //| -SmartPoseFactor LINEARIZE: 0.01 CPU (1000 times, 0.007318 wall, 0.01 children, min: 0 max: 0) /* *************************************************************************/ -TEST( SmartProjectionFactorP, timing ) { +TEST( SmartProjectionRigFactor, timing ) { using namespace vanillaPose; @@ -1095,7 +1095,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropi BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); -TEST(SmartProjectionFactorP, serialize) { +TEST(SmartProjectionRigFactor, serialize) { using namespace vanillaPose; using namespace gtsam::serializationTestHelpers; SmartProjectionParams params; @@ -1108,7 +1108,7 @@ TEST(SmartProjectionFactorP, serialize) { } // SERIALIZATION TEST FAILS: to be fixed -//TEST(SmartProjectionFactorP, serialize2) { +//TEST(SmartProjectionRigFactor, serialize2) { // using namespace vanillaPose; // using namespace gtsam::serializationTestHelpers; // SmartProjectionParams params;