diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 7ac2de8b19..1dcca52448 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -30,8 +30,8 @@ using symbol_shorthand::X; * Unary factor on the unknown pose, resulting from meauring the projection of * a known 3D point in the image */ -class ResectioningFactor: public NoiseModelFactor1 { - typedef NoiseModelFactor1 Base; +class ResectioningFactor: public NoiseModelFactorN { + typedef NoiseModelFactorN Base; Cal3_S2::shared_ptr K_; ///< camera's intrinsic parameters Point3 P_; ///< 3D point on the calibration rig diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index d9b205359e..7a39a4af5a 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -62,10 +62,10 @@ using namespace gtsam; // // The factor will be a unary factor, affect only a single system variable. It will // also use a standard Gaussian noise model. Hence, we will derive our new factor from -// the NoiseModelFactor1. +// the NoiseModelFactorN. #include -class UnaryFactor: public NoiseModelFactor1 { +class UnaryFactor: public NoiseModelFactorN { // The factor will hold a measurement consisting of an (X,Y) location // We could this with a Point2 but here we just use two doubles double mx_, my_; @@ -76,11 +76,11 @@ class UnaryFactor: public NoiseModelFactor1 { // The constructor requires the variable key, the (X, Y) measurement value, and the noise model UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model): - NoiseModelFactor1(model, j), mx_(x), my_(y) {} + NoiseModelFactorN(model, j), mx_(x), my_(y) {} ~UnaryFactor() override {} - // Using the NoiseModelFactor1 base class there are two functions that must be overridden. + // Using the NoiseModelFactorN base class there are two functions that must be overridden. // The first is the 'evaluateError' function. This function implements the desired measurement // function, returning a vector of errors when evaluated at the provided variable value. It // must also calculate the Jacobians for this measurement function, if requested. diff --git a/examples/Pose3SLAMExample_changeKeys.cpp b/examples/Pose3SLAMExample_changeKeys.cpp index 5d4ed6657d..9aa697f140 100644 --- a/examples/Pose3SLAMExample_changeKeys.cpp +++ b/examples/Pose3SLAMExample_changeKeys.cpp @@ -71,11 +71,11 @@ int main(const int argc, const char *argv[]) { if (pose3Between){ Key key1, key2; if(add){ - key1 = pose3Between->key1() + firstKey; - key2 = pose3Between->key2() + firstKey; + key1 = pose3Between->key<1>() + firstKey; + key2 = pose3Between->key<2>() + firstKey; }else{ - key1 = pose3Between->key1() - firstKey; - key2 = pose3Between->key2() - firstKey; + key1 = pose3Between->key<1>() - firstKey; + key2 = pose3Between->key<2>() - firstKey; } NonlinearFactor::shared_ptr simpleFactor( new BetweenFactor(key1, key2, pose3Between->measured(), pose3Between->noiseModel())); diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 7bae41403a..69975b620d 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -69,8 +69,8 @@ namespace br = boost::range; typedef Pose2 Pose; -typedef NoiseModelFactor1 NM1; -typedef NoiseModelFactor2 NM2; +typedef NoiseModelFactorN NM1; +typedef NoiseModelFactorN NM2; typedef BearingRangeFactor BR; double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) { @@ -261,7 +261,7 @@ void runIncremental() if(BetweenFactor::shared_ptr factor = boost::dynamic_pointer_cast >(datasetMeasurements[nextMeasurement])) { - Key key1 = factor->key1(), key2 = factor->key2(); + Key key1 = factor->key<1>(), key2 = factor->key<2>(); if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) { // We found an odometry starting at firstStep firstPose = std::min(key1, key2); @@ -313,11 +313,11 @@ void runIncremental() boost::dynamic_pointer_cast >(measurementf)) { // Stop collecting measurements that are for future steps - if(factor->key1() > step || factor->key2() > step) + if(factor->key<1>() > step || factor->key<2>() > step) break; // Require that one of the nodes is the current one - if(factor->key1() != step && factor->key2() != step) + if(factor->key<1>() != step && factor->key<2>() != step) throw runtime_error("Problem in data file, out-of-sequence measurements"); // Add a new factor @@ -325,22 +325,22 @@ void runIncremental() const auto& measured = factor->measured(); // Initialize the new variable - if(factor->key1() > factor->key2()) { - if(!newVariables.exists(factor->key1())) { // Only need to check newVariables since loop closures come after odometry + if(factor->key<1>() > factor->key<2>()) { + if(!newVariables.exists(factor->key<1>())) { // Only need to check newVariables since loop closures come after odometry if(step == 1) - newVariables.insert(factor->key1(), measured.inverse()); + newVariables.insert(factor->key<1>(), measured.inverse()); else { - Pose prevPose = isam2.calculateEstimate(factor->key2()); - newVariables.insert(factor->key1(), prevPose * measured.inverse()); + Pose prevPose = isam2.calculateEstimate(factor->key<2>()); + newVariables.insert(factor->key<1>(), prevPose * measured.inverse()); } } } else { - if(!newVariables.exists(factor->key2())) { // Only need to check newVariables since loop closures come after odometry + if(!newVariables.exists(factor->key<2>())) { // Only need to check newVariables since loop closures come after odometry if(step == 1) - newVariables.insert(factor->key2(), measured); + newVariables.insert(factor->key<2>(), measured); else { - Pose prevPose = isam2.calculateEstimate(factor->key1()); - newVariables.insert(factor->key2(), prevPose * measured); + Pose prevPose = isam2.calculateEstimate(factor->key<1>()); + newVariables.insert(factor->key<2>(), prevPose * measured); } } } diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index f7eddd4140..b68d6a97cb 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -23,7 +23,7 @@ * void print(const std::string& name) const = 0; * * equality up to tolerance - * tricky to implement, see NoiseModelFactor1 for an example + * tricky to implement, see PriorFactor for an example * equals is not supposed to print out *anything*, just return true|false * bool equals(const Derived& expected, double tol) const = 0; * diff --git a/gtsam/base/types.h b/gtsam/base/types.h index cb8412cdf4..b9f4b0a3e1 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -46,18 +46,49 @@ #include #endif +/* Define macros for ignoring compiler warnings. + * Usage Example: + * ``` + * CLANG_DIAGNOSTIC_PUSH_IGNORE("-Wdeprecated-declarations") + * GCC_DIAGNOSTIC_PUSH_IGNORE("-Wdeprecated-declarations") + * MSVC_DIAGNOSTIC_PUSH_IGNORE(4996) + * // ... code you want to suppress deprecation warnings for ... + * DIAGNOSTIC_POP() + * ``` + */ +#define DO_PRAGMA(x) _Pragma (#x) #ifdef __clang__ # define CLANG_DIAGNOSTIC_PUSH_IGNORE(diag) \ _Pragma("clang diagnostic push") \ - _Pragma("clang diagnostic ignored \"" diag "\"") + DO_PRAGMA(clang diagnostic ignored diag) #else # define CLANG_DIAGNOSTIC_PUSH_IGNORE(diag) #endif -#ifdef __clang__ -# define CLANG_DIAGNOSTIC_POP() _Pragma("clang diagnostic pop") +#ifdef __GNUC__ +# define GCC_DIAGNOSTIC_PUSH_IGNORE(diag) \ + _Pragma("GCC diagnostic push") \ + DO_PRAGMA(GCC diagnostic ignored diag) +#else +# define GCC_DIAGNOSTIC_PUSH_IGNORE(diag) +#endif + +#ifdef _MSC_VER +# define MSVC_DIAGNOSTIC_PUSH_IGNORE(code) \ + _Pragma("warning ( push )") \ + DO_PRAGMA(warning ( disable : code )) +#else +# define MSVC_DIAGNOSTIC_PUSH_IGNORE(code) +#endif + +#if defined(__clang__) +# define DIAGNOSTIC_POP() _Pragma("clang diagnostic pop") +#elif defined(__GNUC__) +# define DIAGNOSTIC_POP() _Pragma("GCC diagnostic pop") +#elif defined(_MSC_VER) +# define DIAGNOSTIC_POP() _Pragma("warning ( pop )") #else -# define CLANG_DIAGNOSTIC_POP() +# define DIAGNOSTIC_POP() #endif namespace gtsam { diff --git a/gtsam/inference/graph-inl.h b/gtsam/inference/graph-inl.h index 3ea66405b7..a23eda60c5 100644 --- a/gtsam/inference/graph-inl.h +++ b/gtsam/inference/graph-inl.h @@ -200,8 +200,8 @@ boost::shared_ptr composePoses(const G& graph, const PredecessorMap boost::shared_ptr factor = boost::dynamic_pointer_cast(nl_factor); if (!factor) continue; - KEY key1 = factor->key1(); - KEY key2 = factor->key2(); + KEY key1 = factor->template key<1>(); + KEY key2 = factor->template key<2>(); PoseVertex v1 = key2vertex.find(key1)->second; PoseVertex v2 = key2vertex.find(key2)->second; @@ -270,8 +270,8 @@ void split(const G& g, const PredecessorMap& tree, G& Ab1, G& Ab2) { FACTOR2>(factor); if (!factor2) continue; - KEY key1 = factor2->key1(); - KEY key2 = factor2->key2(); + KEY key1 = factor2->template key<1>(); + KEY key2 = factor2->template key<2>(); // if the tree contains the key if ((tree.find(key1) != tree.end() && tree.find(key1)->second.compare(key2) == 0) || diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index f4db42d0fd..afaaee1d4e 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -105,8 +105,8 @@ gtsam::NonlinearFactor::shared_ptr AHRSFactor::clone() const { //------------------------------------------------------------------------------ void AHRSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","; + cout << s << "AHRSFactor(" << keyFormatter(this->key<1>()) << "," + << keyFormatter(this->key<2>()) << "," << keyFormatter(this->key<3>()) << ","; _PIM_.print(" preintegrated measurements:"); noiseModel_->print(" noise model: "); } diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index c7d82975a5..87fdd2e918 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -128,10 +128,10 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation } }; -class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3 { +class GTSAM_EXPORT AHRSFactor: public NoiseModelFactorN { typedef AHRSFactor This; - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; PreintegratedAhrsMeasurements _PIM_; @@ -212,6 +212,7 @@ class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3 { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor3", boost::serialization::base_object(*this)); diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index e8da541b1d..93495f227a 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -76,9 +76,9 @@ class AttitudeFactor { * Version of AttitudeFactor for Rot3 * @ingroup navigation */ -class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactor1, public AttitudeFactor { +class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactorN, public AttitudeFactor { - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; public: @@ -132,6 +132,7 @@ class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactor1, public At friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & boost::serialization::make_nvp("AttitudeFactor", @@ -149,10 +150,10 @@ template<> struct traits : public Testable, +class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactorN, public AttitudeFactor { - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; public: @@ -212,6 +213,7 @@ class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactor1, friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & boost::serialization::make_nvp("AttitudeFactor", diff --git a/gtsam/navigation/BarometricFactor.cpp b/gtsam/navigation/BarometricFactor.cpp index 2f0ff7436d..2e87986aec 100644 --- a/gtsam/navigation/BarometricFactor.cpp +++ b/gtsam/navigation/BarometricFactor.cpp @@ -26,8 +26,8 @@ namespace gtsam { void BarometricFactor::print(const string& s, const KeyFormatter& keyFormatter) const { cout << (s.empty() ? "" : s + " ") << "Barometric Factor on " - << keyFormatter(key1()) << "Barometric Bias on " - << keyFormatter(key2()) << "\n"; + << keyFormatter(key<1>()) << "Barometric Bias on " + << keyFormatter(key<2>()) << "\n"; cout << " Baro measurement: " << nT_ << "\n"; noiseModel_->print(" noise model: "); diff --git a/gtsam/navigation/BarometricFactor.h b/gtsam/navigation/BarometricFactor.h index b570049a5e..2d793c475a 100644 --- a/gtsam/navigation/BarometricFactor.h +++ b/gtsam/navigation/BarometricFactor.h @@ -31,9 +31,9 @@ namespace gtsam { * https://www.grc.nasa.gov/www/k-12/airplane/atmosmet.html * @ingroup navigation */ -class GTSAM_EXPORT BarometricFactor : public NoiseModelFactor2 { +class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN { private: - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; double nT_; ///< Height Measurement based on a standard atmosphere @@ -99,6 +99,7 @@ class GTSAM_EXPORT BarometricFactor : public NoiseModelFactor2 { friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility ar& boost::serialization::make_nvp( "NoiseModelFactor1", boost::serialization::base_object(*this)); diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 8d3a7dd315..38b3dc763e 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -204,9 +204,9 @@ gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const { void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { cout << (s.empty() ? s : s + "\n") << "CombinedImuFactor(" - << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," << keyFormatter(this->key4()) << "," - << keyFormatter(this->key5()) << "," << keyFormatter(this->key6()) + << keyFormatter(this->key<1>()) << "," << keyFormatter(this->key<2>()) << "," + << keyFormatter(this->key<3>()) << "," << keyFormatter(this->key<4>()) << "," + << keyFormatter(this->key<5>()) << "," << keyFormatter(this->key<6>()) << ")\n"; _PIM_.print(" preintegrated measurements:"); this->noiseModel_->print(" noise model: "); diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 5591bb357d..3448e77948 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -255,14 +255,14 @@ class GTSAM_EXPORT PreintegratedCombinedMeasurements : public PreintegrationType * * @ingroup navigation */ -class GTSAM_EXPORT CombinedImuFactor: public NoiseModelFactor6 { public: private: typedef CombinedImuFactor This; - typedef NoiseModelFactor6 Base; PreintegratedCombinedMeasurements _PIM_; @@ -334,7 +334,9 @@ class GTSAM_EXPORT CombinedImuFactor: public NoiseModelFactor6 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor6); + // NoiseModelFactor6 instead of NoiseModelFactorN for backward compatibility + ar& boost::serialization::make_nvp( + "NoiseModelFactor6", boost::serialization::base_object(*this)); ar& BOOST_SERIALIZATION_NVP(_PIM_); } diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h index 6ab4c2f025..4db2b82c0c 100644 --- a/gtsam/navigation/ConstantVelocityFactor.h +++ b/gtsam/navigation/ConstantVelocityFactor.h @@ -26,15 +26,15 @@ namespace gtsam { * Binary factor for applying a constant velocity model to a moving body represented as a NavState. * The only measurement is dt, the time delta between the states. */ -class ConstantVelocityFactor : public NoiseModelFactor2 { +class ConstantVelocityFactor : public NoiseModelFactorN { double dt_; public: - using Base = NoiseModelFactor2; + using Base = NoiseModelFactorN; public: ConstantVelocityFactor(Key i, Key j, double dt, const SharedNoiseModel &model) - : NoiseModelFactor2(model, i, j), dt_(dt) {} + : NoiseModelFactorN(model, i, j), dt_(dt) {} ~ConstantVelocityFactor() override{}; /** diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index 40d0ef22a5..e515d90122 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -32,11 +32,11 @@ namespace gtsam { * See Farrell08book or e.g. http://www.dirsig.org/docs/new/coordinates.html * @ingroup navigation */ -class GTSAM_EXPORT GPSFactor: public NoiseModelFactor1 { +class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN { private: - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; Point3 nT_; ///< Position measurement in cartesian coordinates @@ -99,6 +99,7 @@ class GTSAM_EXPORT GPSFactor: public NoiseModelFactor1 { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); @@ -110,11 +111,11 @@ class GTSAM_EXPORT GPSFactor: public NoiseModelFactor1 { * Version of GPSFactor for NavState * @ingroup navigation */ -class GTSAM_EXPORT GPSFactor2: public NoiseModelFactor1 { +class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN { private: - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; Point3 nT_; ///< Position measurement in cartesian coordinates @@ -163,6 +164,7 @@ class GTSAM_EXPORT GPSFactor2: public NoiseModelFactor1 { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 9b6affaaf8..1b07991e93 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -133,9 +133,9 @@ std::ostream& operator<<(std::ostream& os, const ImuFactor& f) { //------------------------------------------------------------------------------ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << (s.empty() ? s : s + "\n") << "ImuFactor(" << keyFormatter(this->key1()) - << "," << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) - << "," << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) + cout << (s.empty() ? s : s + "\n") << "ImuFactor(" << keyFormatter(this->key<1>()) + << "," << keyFormatter(this->key<2>()) << "," << keyFormatter(this->key<3>()) + << "," << keyFormatter(this->key<4>()) << "," << keyFormatter(this->key<5>()) << ")\n"; cout << *this << endl; } @@ -184,22 +184,22 @@ PreintegratedImuMeasurements ImuFactor::Merge( ImuFactor::shared_ptr ImuFactor::Merge(const shared_ptr& f01, const shared_ptr& f12) { // IMU bias keys must be the same. - if (f01->key5() != f12->key5()) + if (f01->key<5>() != f12->key<5>()) throw std::domain_error("ImuFactor::Merge: IMU bias keys must be the same"); // expect intermediate pose, velocity keys to matchup. - if (f01->key3() != f12->key1() || f01->key4() != f12->key2()) + if (f01->key<3>() != f12->key<1>() || f01->key<4>() != f12->key<2>()) throw std::domain_error( "ImuFactor::Merge: intermediate pose, velocity keys need to match up"); // return new factor auto pim02 = Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements()); - return boost::make_shared(f01->key1(), // P0 - f01->key2(), // V0 - f12->key3(), // P2 - f12->key4(), // V2 - f01->key5(), // B + return boost::make_shared(f01->key<1>(), // P0 + f01->key<2>(), // V0 + f12->key<3>(), // P2 + f12->key<4>(), // V2 + f01->key<5>(), // B pim02); } #endif @@ -230,8 +230,8 @@ std::ostream& operator<<(std::ostream& os, const ImuFactor2& f) { void ImuFactor2::print(const string& s, const KeyFormatter& keyFormatter) const { cout << (s.empty() ? s : s + "\n") << "ImuFactor2(" - << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << ")\n"; + << keyFormatter(this->key<1>()) << "," << keyFormatter(this->key<2>()) << "," + << keyFormatter(this->key<3>()) << ")\n"; cout << *this << endl; } diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index bffe3e6455..feae1eb140 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -168,12 +168,12 @@ class GTSAM_EXPORT PreintegratedImuMeasurements: public PreintegrationType { * * @ingroup navigation */ -class GTSAM_EXPORT ImuFactor: public NoiseModelFactor5 { private: typedef ImuFactor This; - typedef NoiseModelFactor5 Base; PreintegratedImuMeasurements _PIM_; @@ -248,6 +248,7 @@ class GTSAM_EXPORT ImuFactor: public NoiseModelFactor5 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor5 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor5", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(_PIM_); @@ -259,11 +260,11 @@ class GTSAM_EXPORT ImuFactor: public NoiseModelFactor5 { +class GTSAM_EXPORT ImuFactor2 : public NoiseModelFactorN { private: typedef ImuFactor2 This; - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; PreintegratedImuMeasurements _PIM_; @@ -316,6 +317,7 @@ class GTSAM_EXPORT ImuFactor2 : public NoiseModelFactor3 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor3", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(_PIM_); diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index 895ac6512c..9a718a5e1a 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -30,7 +30,7 @@ namespace gtsam { * and assumes scale, direction, and the bias are given. * Rotation is around negative Z axis, i.e. positive is yaw to right! */ -class MagFactor: public NoiseModelFactor1 { +class MagFactor: public NoiseModelFactorN { const Point3 measured_; ///< The measured magnetometer values const Point3 nM_; ///< Local magnetic field (mag output units) @@ -50,7 +50,7 @@ class MagFactor: public NoiseModelFactor1 { MagFactor(Key key, const Point3& measured, double scale, const Unit3& direction, const Point3& bias, const SharedNoiseModel& model) : - NoiseModelFactor1(model, key), // + NoiseModelFactorN(model, key), // measured_(measured), nM_(scale * direction), bias_(bias) { } @@ -87,7 +87,7 @@ class MagFactor: public NoiseModelFactor1 { * This version uses model measured bM = scale * bRn * direction + bias * and assumes scale, direction, and the bias are given */ -class MagFactor1: public NoiseModelFactor1 { +class MagFactor1: public NoiseModelFactorN { const Point3 measured_; ///< The measured magnetometer values const Point3 nM_; ///< Local magnetic field (mag output units) @@ -99,7 +99,7 @@ class MagFactor1: public NoiseModelFactor1 { MagFactor1(Key key, const Point3& measured, double scale, const Unit3& direction, const Point3& bias, const SharedNoiseModel& model) : - NoiseModelFactor1(model, key), // + NoiseModelFactorN(model, key), // measured_(measured), nM_(scale * direction), bias_(bias) { } @@ -125,7 +125,7 @@ class MagFactor1: public NoiseModelFactor1 { * This version uses model measured bM = bRn * nM + bias * and optimizes for both nM and the bias, where nM is in units defined by magnetometer */ -class MagFactor2: public NoiseModelFactor2 { +class MagFactor2: public NoiseModelFactorN { const Point3 measured_; ///< The measured magnetometer values const Rot3 bRn_; ///< The assumed known rotation from nav to body @@ -135,7 +135,7 @@ class MagFactor2: public NoiseModelFactor2 { /** Constructor */ MagFactor2(Key key1, Key key2, const Point3& measured, const Rot3& nRb, const SharedNoiseModel& model) : - NoiseModelFactor2(model, key1, key2), // + NoiseModelFactorN(model, key1, key2), // measured_(measured), bRn_(nRb.inverse()) { } @@ -166,7 +166,7 @@ class MagFactor2: public NoiseModelFactor2 { * This version uses model measured bM = scale * bRn * direction + bias * and optimizes for both scale, direction, and the bias. */ -class MagFactor3: public NoiseModelFactor3 { +class MagFactor3: public NoiseModelFactorN { const Point3 measured_; ///< The measured magnetometer values const Rot3 bRn_; ///< The assumed known rotation from nav to body @@ -176,7 +176,7 @@ class MagFactor3: public NoiseModelFactor3 { /** Constructor */ MagFactor3(Key key1, Key key2, Key key3, const Point3& measured, const Rot3& nRb, const SharedNoiseModel& model) : - NoiseModelFactor3(model, key1, key2, key3), // + NoiseModelFactorN(model, key1, key2, key3), // measured_(measured), bRn_(nRb.inverse()) { } diff --git a/gtsam/navigation/MagPoseFactor.h b/gtsam/navigation/MagPoseFactor.h index da2a54ce99..c817e22b43 100644 --- a/gtsam/navigation/MagPoseFactor.h +++ b/gtsam/navigation/MagPoseFactor.h @@ -25,10 +25,10 @@ namespace gtsam { * expressed in the sensor frame. */ template -class MagPoseFactor: public NoiseModelFactor1 { +class MagPoseFactor: public NoiseModelFactorN { private: using This = MagPoseFactor; - using Base = NoiseModelFactor1; + using Base = NoiseModelFactorN; using Point = typename POSE::Translation; ///< Could be a Vector2 or Vector3 depending on POSE. using Rot = typename POSE::Rotation; @@ -129,6 +129,7 @@ class MagPoseFactor: public NoiseModelFactor1 { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h index dc51de7bbb..d76a6ea7e6 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter.h @@ -53,8 +53,8 @@ class ExtendedKalmanFilter { #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 //@deprecated: any NoiseModelFactor will do, as long as they have the right keys - typedef NoiseModelFactor2 MotionFactor; - typedef NoiseModelFactor1 MeasurementFactor; + typedef NoiseModelFactorN MotionFactor; + typedef NoiseModelFactorN MeasurementFactor; #endif protected: diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h index 394b22b6b7..1fb44ebf77 100644 --- a/gtsam/nonlinear/FunctorizedFactor.h +++ b/gtsam/nonlinear/FunctorizedFactor.h @@ -56,9 +56,9 @@ namespace gtsam { * MultiplyFunctor(multiplier)); */ template -class FunctorizedFactor : public NoiseModelFactor1 { +class FunctorizedFactor : public NoiseModelFactorN { private: - using Base = NoiseModelFactor1; + using Base = NoiseModelFactorN; R measured_; ///< value that is compared with functor return value SharedNoiseModel noiseModel_; ///< noise model @@ -101,7 +101,7 @@ class FunctorizedFactor : public NoiseModelFactor1 { const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override { Base::print(s, keyFormatter); std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor(" - << keyFormatter(this->key()) << ")" << std::endl; + << keyFormatter(this->template key<1>()) << ")" << std::endl; traits::Print(measured_, " measurement: "); std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose() << std::endl; @@ -120,6 +120,7 @@ class FunctorizedFactor : public NoiseModelFactor1 { friend class boost::serialization::access; template void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility ar &boost::serialization::make_nvp( "NoiseModelFactor1", boost::serialization::base_object(*this)); ar &BOOST_SERIALIZATION_NVP(measured_); @@ -155,9 +156,9 @@ FunctorizedFactor MakeFunctorizedFactor(Key key, const R &z, * @param T2: The second argument type for the functor. */ template -class FunctorizedFactor2 : public NoiseModelFactor2 { +class FunctorizedFactor2 : public NoiseModelFactorN { private: - using Base = NoiseModelFactor2; + using Base = NoiseModelFactorN; R measured_; ///< value that is compared with functor return value SharedNoiseModel noiseModel_; ///< noise model @@ -207,8 +208,8 @@ class FunctorizedFactor2 : public NoiseModelFactor2 { const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override { Base::print(s, keyFormatter); std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor2(" - << keyFormatter(this->key1()) << ", " - << keyFormatter(this->key2()) << ")" << std::endl; + << keyFormatter(this->template key<1>()) << ", " + << keyFormatter(this->template key<2>()) << ")" << std::endl; traits::Print(measured_, " measurement: "); std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose() << std::endl; @@ -227,6 +228,7 @@ class FunctorizedFactor2 : public NoiseModelFactor2 { friend class boost::serialization::access; template void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility ar &boost::serialization::make_nvp( "NoiseModelFactor2", boost::serialization::base_object(*this)); ar &BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 43d30254ef..d1aa58b27a 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -42,7 +42,7 @@ namespace gtsam { * \nosubgrouping */ template -class NonlinearEquality: public NoiseModelFactor1 { +class NonlinearEquality: public NoiseModelFactorN { public: typedef VALUE T; @@ -62,7 +62,7 @@ class NonlinearEquality: public NoiseModelFactor1 { using This = NonlinearEquality; // typedef to base class - using Base = NoiseModelFactor1; + using Base = NoiseModelFactorN; public: @@ -184,6 +184,7 @@ class NonlinearEquality: public NoiseModelFactor1 { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); @@ -203,13 +204,13 @@ struct traits> : Testable> {}; * Simple unary equality constraint - fixes a value for a variable */ template -class NonlinearEquality1: public NoiseModelFactor1 { +class NonlinearEquality1: public NoiseModelFactorN { public: typedef VALUE X; protected: - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; typedef NonlinearEquality1 This; /// Default constructor to allow for serialization @@ -272,6 +273,7 @@ class NonlinearEquality1: public NoiseModelFactor1 { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); @@ -290,9 +292,9 @@ struct traits > * be the same. */ template -class NonlinearEquality2 : public NoiseModelFactor2 { +class NonlinearEquality2 : public NoiseModelFactorN { protected: - using Base = NoiseModelFactor2; + using Base = NoiseModelFactorN; using This = NonlinearEquality2; GTSAM_CONCEPT_MANIFOLD_TYPE(T) @@ -337,6 +339,7 @@ class NonlinearEquality2 : public NoiseModelFactor2 { friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility ar& boost::serialization::make_nvp( "NoiseModelFactor2", boost::serialization::base_object(*this)); } diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index ad52355e42..c58179db35 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -14,6 +14,7 @@ * @brief Non-linear factor base classes * @author Frank Dellaert * @author Richard Roberts + * @author Gerry Chen */ // \callgraph @@ -431,8 +432,9 @@ class NoiseModelFactorN : public NoiseModelFactor { * // key<3>() // ERROR! Will not compile * ``` */ - template > + template inline Key key() const { + static_assert(I <= N, "Index out of bounds"); return keys_[I - 1]; } @@ -492,6 +494,7 @@ class NoiseModelFactorN : public NoiseModelFactor { OptionalMatrix... H) const = 0; /// @} + /// @name Convenience method overloads /// @{ @@ -550,11 +553,54 @@ class NoiseModelFactorN : public NoiseModelFactor { ar& boost::serialization::make_nvp( "NoiseModelFactor", boost::serialization::base_object(*this)); } + + public: + /// @name Deprecated methods. Use `key<1>()`, `key<2>()`, ... instead of old + /// `key1()`, `key2()`, ... + /// If your class is templated AND you are trying to call `key<1>` inside your + /// class, due to dependent types you need to do `this->template key<1>()`. + /// @{ + + inline Key GTSAM_DEPRECATED key1() const { + return key<1>(); + } + template + inline Key GTSAM_DEPRECATED key2() const { + static_assert(I <= N, "Index out of bounds"); + return key<2>(); + } + template + inline Key GTSAM_DEPRECATED key3() const { + static_assert(I <= N, "Index out of bounds"); + return key<3>(); + } + template + inline Key GTSAM_DEPRECATED key4() const { + static_assert(I <= N, "Index out of bounds"); + return key<4>(); + } + template + inline Key GTSAM_DEPRECATED key5() const { + static_assert(I <= N, "Index out of bounds"); + return key<5>(); + } + template + inline Key GTSAM_DEPRECATED key6() const { + static_assert(I <= N, "Index out of bounds"); + return key<6>(); + } + + /// @} + }; // \class NoiseModelFactorN /* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key() with .key<1> and X1 +/** @deprecated: use NoiseModelFactorN, replacing .key() with .key<1>() and X1 * with ValueType<1>. + * If your class is templated AND you are trying to call `.key<1>()` or + * `ValueType<1>` inside your class, due to dependent types you need to do + * `this->template key<1>()` or `This::template ValueType<1>`. + * ~~~ * A convenient base class for creating your own NoiseModelFactor * with 1 variable. To derive from this class, implement evaluateError(). */ @@ -578,11 +624,6 @@ class GTSAM_DEPRECATED NoiseModelFactor1 : public NoiseModelFactorN { using NoiseModelFactorN::NoiseModelFactorN; ~NoiseModelFactor1() override {} - /** Method to retrieve key. - * Similar to `ValueType`, you can probably do `return key<1>();` - */ - inline Key key() const { return NoiseModelFactorN::template key<1>(); } - private: /** Serialization function */ friend class boost::serialization::access; @@ -594,8 +635,12 @@ class GTSAM_DEPRECATED NoiseModelFactor1 : public NoiseModelFactorN { }; // \class NoiseModelFactor1 /* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1>() and X1 * with ValueType<1>. + * If your class is templated AND you are trying to call `.key<1>()` or + * `ValueType<1>` inside your class, due to dependent types you need to do + * `this->template key<1>()` or `This::template ValueType<1>`. + * ~~~ * A convenient base class for creating your own NoiseModelFactor * with 2 variables. To derive from this class, implement evaluateError(). */ @@ -621,12 +666,6 @@ class GTSAM_DEPRECATED NoiseModelFactor2 using NoiseModelFactorN::NoiseModelFactorN; ~NoiseModelFactor2() override {} - /** Methods to retrieve keys. - * Similar to `ValueType`, you can probably do `return key<#>();` - */ - inline Key key1() const { return this->template key<1>(); } - inline Key key2() const { return this->template key<2>(); } - private: /** Serialization function */ friend class boost::serialization::access; @@ -638,8 +677,12 @@ class GTSAM_DEPRECATED NoiseModelFactor2 }; // \class NoiseModelFactor2 /* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1>() and X1 * with ValueType<1>. + * If your class is templated AND you are trying to call `.key<1>()` or + * `ValueType<1>` inside your class, due to dependent types you need to do + * `this->template key<1>()` or `This::template ValueType<1>`. + * ~~~ * A convenient base class for creating your own NoiseModelFactor * with 3 variables. To derive from this class, implement evaluateError(). */ @@ -666,13 +709,6 @@ class GTSAM_DEPRECATED NoiseModelFactor3 using NoiseModelFactorN::NoiseModelFactorN; ~NoiseModelFactor3() override {} - /** Methods to retrieve keys. - * Similar to `ValueType`, you can probably do `return key<#>();` - */ - inline Key key1() const { return this->template key<1>(); } - inline Key key2() const { return this->template key<2>(); } - inline Key key3() const { return this->template key<3>(); } - private: /** Serialization function */ friend class boost::serialization::access; @@ -684,8 +720,12 @@ class GTSAM_DEPRECATED NoiseModelFactor3 }; // \class NoiseModelFactor3 /* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1>() and X1 * with ValueType<1>. + * If your class is templated AND you are trying to call `.key<1>()` or + * `ValueType<1>` inside your class, due to dependent types you need to do + * `this->template key<1>()` or `This::template ValueType<1>`. + * ~~~ * A convenient base class for creating your own NoiseModelFactor * with 4 variables. To derive from this class, implement evaluateError(). */ @@ -713,14 +753,6 @@ class GTSAM_DEPRECATED NoiseModelFactor4 using NoiseModelFactorN::NoiseModelFactorN; ~NoiseModelFactor4() override {} - /** Methods to retrieve keys. - * Similar to `ValueType`, you can probably do `return key<#>();` - */ - inline Key key1() const { return this->template key<1>(); } - inline Key key2() const { return this->template key<2>(); } - inline Key key3() const { return this->template key<3>(); } - inline Key key4() const { return this->template key<4>(); } - private: /** Serialization function */ friend class boost::serialization::access; @@ -732,8 +764,12 @@ class GTSAM_DEPRECATED NoiseModelFactor4 }; // \class NoiseModelFactor4 /* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1>() and X1 * with ValueType<1>. + * If your class is templated AND you are trying to call `.key<1>()` or + * `ValueType<1>` inside your class, due to dependent types you need to do + * `this->template key<1>()` or `This::template ValueType<1>`. + * ~~~ * A convenient base class for creating your own NoiseModelFactor * with 5 variables. To derive from this class, implement evaluateError(). */ @@ -763,15 +799,6 @@ class GTSAM_DEPRECATED NoiseModelFactor5 VALUE5>::NoiseModelFactorN; ~NoiseModelFactor5() override {} - /** Methods to retrieve keys. - * Similar to `ValueType`, you can probably do `return key<#>();` - */ - inline Key key1() const { return this->template key<1>(); } - inline Key key2() const { return this->template key<2>(); } - inline Key key3() const { return this->template key<3>(); } - inline Key key4() const { return this->template key<4>(); } - inline Key key5() const { return this->template key<5>(); } - private: /** Serialization function */ friend class boost::serialization::access; @@ -783,8 +810,12 @@ class GTSAM_DEPRECATED NoiseModelFactor5 }; // \class NoiseModelFactor5 /* ************************************************************************* */ -/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1> and X1 +/** @deprecated: use NoiseModelFactorN, replacing .key1() with .key<1>() and X1 * with ValueType<1>. + * If your class is templated AND you are trying to call `.key<1>()` or + * `ValueType<1>` inside your class, due to dependent types you need to do + * `this->template key<1>()` or `This::template ValueType<1>`. + * ~~~ * A convenient base class for creating your own NoiseModelFactor * with 6 variables. To derive from this class, implement evaluateError(). */ @@ -817,16 +848,6 @@ class GTSAM_DEPRECATED NoiseModelFactor6 VALUE6>::NoiseModelFactorN; ~NoiseModelFactor6() override {} - /** Methods to retrieve keys. - * Similar to `ValueType`, you can probably do `return key<#>();` - */ - inline Key key1() const { return this->template key<1>(); } - inline Key key2() const { return this->template key<2>(); } - inline Key key3() const { return this->template key<3>(); } - inline Key key4() const { return this->template key<4>(); } - inline Key key5() const { return this->template key<5>(); } - inline Key key6() const { return this->template key<6>(); } - private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h index 2d4a0ca326..d81ffbd319 100644 --- a/gtsam/nonlinear/PriorFactor.h +++ b/gtsam/nonlinear/PriorFactor.h @@ -27,14 +27,14 @@ namespace gtsam { * @ingroup nonlinear */ template - class PriorFactor: public NoiseModelFactor1 { + class PriorFactor: public NoiseModelFactorN { public: typedef VALUE T; private: - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; VALUE prior_; /** The measurement */ @@ -105,6 +105,7 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(prior_); diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 58e98ebfa9..c00669e365 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -958,7 +958,7 @@ static BinaryMeasurement convertPose2ToBinaryMeasurementRot2( // the (2,2) entry of Pose2's covariance corresponds to Rot2's covariance // because the tangent space of Pose2 is ordered as (vx, vy, w) auto model = noiseModel::Isotropic::Variance(1, M(2, 2)); - return BinaryMeasurement(f->key1(), f->key2(), f->measured().rotation(), + return BinaryMeasurement(f->key<1>(), f->key<2>(), f->measured().rotation(), model); } @@ -1006,7 +1006,7 @@ static BinaryMeasurement convert( // the upper-left 3x3 sub-block of Pose3's covariance corresponds to Rot3's covariance // because the tangent space of Pose3 is ordered as (w,T) where w and T are both Vector3's auto model = noiseModel::Gaussian::Covariance(M.block<3, 3>(0, 0)); - return BinaryMeasurement(f->key1(), f->key2(), f->measured().rotation(), + return BinaryMeasurement(f->key<1>(), f->key<2>(), f->measured().rotation(), model); } diff --git a/gtsam/sfm/ShonanFactor.cpp b/gtsam/sfm/ShonanFactor.cpp index b911fb5a47..a48b6e6fa7 100644 --- a/gtsam/sfm/ShonanFactor.cpp +++ b/gtsam/sfm/ShonanFactor.cpp @@ -35,7 +35,7 @@ template ShonanFactor::ShonanFactor(Key j1, Key j2, const Rot &R12, size_t p, const SharedNoiseModel &model, const boost::shared_ptr &G) - : NoiseModelFactor2(ConvertNoiseModel(model, p * d), j1, j2), + : NoiseModelFactorN(ConvertNoiseModel(model, p * d), j1, j2), M_(R12.matrix()), // d*d in all cases p_(p), // 4 for SO(4) pp_(p * p), // 16 for SO(4) @@ -57,8 +57,8 @@ ShonanFactor::ShonanFactor(Key j1, Key j2, const Rot &R12, size_t p, template void ShonanFactor::print(const std::string &s, const KeyFormatter &keyFormatter) const { - std::cout << s << "ShonanFactor<" << p_ << ">(" << keyFormatter(key1()) << "," - << keyFormatter(key2()) << ")\n"; + std::cout << s << "ShonanFactor<" << p_ << ">(" << keyFormatter(key<1>()) << "," + << keyFormatter(key<2>()) << ")\n"; traits::Print(M_, " M: "); noiseModel_->print(" noise model: "); } @@ -68,7 +68,7 @@ template bool ShonanFactor::equals(const NonlinearFactor &expected, double tol) const { auto e = dynamic_cast(&expected); - return e != nullptr && NoiseModelFactor2::equals(*e, tol) && + return e != nullptr && NoiseModelFactorN::equals(*e, tol) && p_ == e->p_ && M_ == e->M_; } diff --git a/gtsam/sfm/ShonanFactor.h b/gtsam/sfm/ShonanFactor.h index 3c43c2c52a..78cc397656 100644 --- a/gtsam/sfm/ShonanFactor.h +++ b/gtsam/sfm/ShonanFactor.h @@ -33,7 +33,7 @@ namespace gtsam { * the SO(p) matrices down to a Stiefel manifold of p*d matrices. */ template -class GTSAM_EXPORT ShonanFactor : public NoiseModelFactor2 { +class GTSAM_EXPORT ShonanFactor : public NoiseModelFactorN { Matrix M_; ///< measured rotation between R1 and R2 size_t p_, pp_; ///< dimensionality constants boost::shared_ptr G_; ///< matrix of vectorized generators @@ -66,7 +66,7 @@ class GTSAM_EXPORT ShonanFactor : public NoiseModelFactor2 { double tol = 1e-9) const override; /// @} - /// @name NoiseModelFactor2 methods + /// @name NoiseModelFactorN methods /// @{ /// Error is Frobenius norm between Q1*P*R12 and Q2*P, where P=[I_3x3;0] diff --git a/gtsam/sfm/TranslationFactor.h b/gtsam/sfm/TranslationFactor.h index 735746b3a0..8850d7d2d0 100644 --- a/gtsam/sfm/TranslationFactor.h +++ b/gtsam/sfm/TranslationFactor.h @@ -39,9 +39,9 @@ namespace gtsam { * * */ -class TranslationFactor : public NoiseModelFactor2 { +class TranslationFactor : public NoiseModelFactorN { private: - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; Point3 measured_w_aZb_; public: diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index dc72f07b22..f2b41e0a1c 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -37,7 +37,7 @@ namespace gtsam { * @ingroup slam */ template - class BetweenFactor: public NoiseModelFactor2 { + class BetweenFactor: public NoiseModelFactorN { // Check that VALUE type is a testable Lie group BOOST_CONCEPT_ASSERT((IsTestable)); @@ -50,7 +50,7 @@ namespace gtsam { private: typedef BetweenFactor This; - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; VALUE measured_; /** The measurement */ @@ -88,8 +88,8 @@ namespace gtsam { const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "BetweenFactor(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << ")\n"; + << keyFormatter(this->template key<1>()) << "," + << keyFormatter(this->template key<2>()) << ")\n"; traits::Print(measured_, " measured: "); this->noiseModel_->print(" noise model: "); } @@ -101,7 +101,7 @@ namespace gtsam { } /// @} - /// @name NoiseModelFactor2 methods + /// @name NoiseModelFactorN methods /// @{ /// evaluate error, returns vector of errors size of tangent space @@ -136,6 +136,7 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index c09d4136d4..79890ec944 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -30,9 +30,9 @@ namespace gtsam { * @ingroup slam */ template -struct BoundingConstraint1: public NoiseModelFactor1 { +struct BoundingConstraint1: public NoiseModelFactorN { typedef VALUE X; - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; typedef boost::shared_ptr > shared_ptr; double threshold_; @@ -85,6 +85,7 @@ struct BoundingConstraint1: public NoiseModelFactor1 { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(threshold_); @@ -97,11 +98,11 @@ struct BoundingConstraint1: public NoiseModelFactor1 { * to implement for specific systems */ template -struct BoundingConstraint2: public NoiseModelFactor2 { +struct BoundingConstraint2: public NoiseModelFactorN { typedef VALUE1 X1; typedef VALUE2 X2; - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; typedef boost::shared_ptr > shared_ptr; double threshold_; @@ -128,7 +129,7 @@ struct BoundingConstraint2: public NoiseModelFactor2 { /** active when constraint *NOT* met */ bool active(const Values& c) const override { // note: still active at equality to avoid zigzagging - double x = value(c.at(this->key1()), c.at(this->key2())); + double x = value(c.at(this->template key<1>()), c.at(this->template key<2>())); return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; } @@ -158,6 +159,7 @@ struct BoundingConstraint2: public NoiseModelFactor2 { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(threshold_); diff --git a/gtsam/slam/EssentialMatrixConstraint.cpp b/gtsam/slam/EssentialMatrixConstraint.cpp index 5397c2e57d..c1f8b286bd 100644 --- a/gtsam/slam/EssentialMatrixConstraint.cpp +++ b/gtsam/slam/EssentialMatrixConstraint.cpp @@ -27,8 +27,8 @@ namespace gtsam { /* ************************************************************************* */ void EssentialMatrixConstraint::print(const std::string& s, const KeyFormatter& keyFormatter) const { - std::cout << s << "EssentialMatrixConstraint(" << keyFormatter(this->key1()) - << "," << keyFormatter(this->key2()) << ")\n"; + std::cout << s << "EssentialMatrixConstraint(" << keyFormatter(this->key<1>()) + << "," << keyFormatter(this->key<2>()) << ")\n"; measuredE_.print(" measured: "); this->noiseModel_->print(" noise model: "); } diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h index dd3c522861..9d84dfa7be 100644 --- a/gtsam/slam/EssentialMatrixConstraint.h +++ b/gtsam/slam/EssentialMatrixConstraint.h @@ -27,12 +27,12 @@ namespace gtsam { * Binary factor between two Pose3 variables induced by an EssentialMatrix measurement * @ingroup slam */ -class GTSAM_EXPORT EssentialMatrixConstraint: public NoiseModelFactor2 { +class GTSAM_EXPORT EssentialMatrixConstraint: public NoiseModelFactorN { private: typedef EssentialMatrixConstraint This; - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; EssentialMatrix measuredE_; /** The measurement is an essential matrix */ @@ -93,6 +93,7 @@ class GTSAM_EXPORT EssentialMatrixConstraint: public NoiseModelFactor2 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 5997ad2247..8a0277a459 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -31,10 +31,10 @@ namespace gtsam { /** * Factor that evaluates epipolar error p'Ep for given essential matrix */ -class EssentialMatrixFactor : public NoiseModelFactor1 { +class EssentialMatrixFactor : public NoiseModelFactorN { Vector3 vA_, vB_; ///< Homogeneous versions, in ideal coordinates - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; typedef EssentialMatrixFactor This; public: @@ -106,12 +106,12 @@ class EssentialMatrixFactor : public NoiseModelFactor1 { * in image 2 is perfect, and returns re-projection error in image 1 */ class EssentialMatrixFactor2 - : public NoiseModelFactor2 { + : public NoiseModelFactorN { Point3 dP1_; ///< 3D point corresponding to measurement in image 1 Point2 pn_; ///< Measurement in image 2, in ideal coordinates double f_; ///< approximate conversion factor for error scaling - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; typedef EssentialMatrixFactor2 This; public: @@ -321,11 +321,11 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 { */ template class EssentialMatrixFactor4 - : public NoiseModelFactor2 { + : public NoiseModelFactorN { private: Point2 pA_, pB_; ///< points in pixel coordinates - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; typedef EssentialMatrixFactor4 This; static constexpr int DimK = FixedDimension::value; diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h index 05e23ce6de..60f880a7a9 100644 --- a/gtsam/slam/FrobeniusFactor.h +++ b/gtsam/slam/FrobeniusFactor.h @@ -48,7 +48,7 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t n, * element of SO(3) or SO(4). */ template -class FrobeniusPrior : public NoiseModelFactor1 { +class FrobeniusPrior : public NoiseModelFactorN { enum { Dim = Rot::VectorN2::RowsAtCompileTime }; using MatrixNN = typename Rot::MatrixNN; Eigen::Matrix vecM_; ///< vectorized matrix to approximate @@ -59,7 +59,7 @@ class FrobeniusPrior : public NoiseModelFactor1 { /// Constructor FrobeniusPrior(Key j, const MatrixNN& M, const SharedNoiseModel& model = nullptr) - : NoiseModelFactor1(ConvertNoiseModel(model, Dim), j) { + : NoiseModelFactorN(ConvertNoiseModel(model, Dim), j) { vecM_ << Eigen::Map(M.data(), Dim, 1); } @@ -75,13 +75,13 @@ class FrobeniusPrior : public NoiseModelFactor1 { * The template argument can be any fixed-size SO. */ template -class FrobeniusFactor : public NoiseModelFactor2 { +class FrobeniusFactor : public NoiseModelFactorN { enum { Dim = Rot::VectorN2::RowsAtCompileTime }; public: /// Constructor FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr) - : NoiseModelFactor2(ConvertNoiseModel(model, Dim), j1, + : NoiseModelFactorN(ConvertNoiseModel(model, Dim), j1, j2) {} /// Error is just Frobenius norm between rotation matrices. @@ -101,7 +101,7 @@ class FrobeniusFactor : public NoiseModelFactor2 { * and in fact only SO3 and SO4 really work, as we need SO::AdjointMap. */ template -class FrobeniusBetweenFactor : public NoiseModelFactor2 { +class FrobeniusBetweenFactor : public NoiseModelFactorN { Rot R12_; ///< measured rotation between R1 and R2 Eigen::Matrix R2hat_H_R1_; ///< fixed derivative of R2hat wrpt R1 @@ -116,7 +116,7 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2 { /// Construct from two keys and measured rotation FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12, const SharedNoiseModel& model = nullptr) - : NoiseModelFactor2( + : NoiseModelFactorN( ConvertNoiseModel(model, Dim), j1, j2), R12_(R12), R2hat_H_R1_(R12.inverse().AdjointMap()) {} @@ -130,8 +130,8 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2 { print(const std::string &s, const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "FrobeniusBetweenFactor<" << demangle(typeid(Rot).name()) - << ">(" << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << ")\n"; + << ">(" << keyFormatter(this->template key<1>()) << "," + << keyFormatter(this->template key<2>()) << ")\n"; traits::Print(R12_, " R12: "); this->noiseModel_->print(" noise model: "); } @@ -140,12 +140,12 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2 { bool equals(const NonlinearFactor &expected, double tol = 1e-9) const override { auto e = dynamic_cast(&expected); - return e != nullptr && NoiseModelFactor2::equals(*e, tol) && + return e != nullptr && NoiseModelFactorN::equals(*e, tol) && traits::Equals(this->R12_, e->R12_, tol); } /// @} - /// @name NoiseModelFactor2 methods + /// @name NoiseModelFactorN methods /// @{ /// Error is Frobenius norm between R1*R12 and R2. diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 23f10de343..f3089bd71f 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -57,7 +57,7 @@ namespace gtsam { * @ingroup slam */ template -class GeneralSFMFactor: public NoiseModelFactor2 { +class GeneralSFMFactor: public NoiseModelFactorN { GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA) GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK) @@ -74,7 +74,7 @@ class GeneralSFMFactor: public NoiseModelFactor2 { public: typedef GeneralSFMFactor This;///< typedef for this object - typedef NoiseModelFactor2 Base;///< typedef for the base class + typedef NoiseModelFactorN Base;///< typedef for the base class // shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -140,7 +140,7 @@ class GeneralSFMFactor: public NoiseModelFactor2 { // Only linearize if the factor is active if (!this->active(values)) return boost::shared_ptr(); - const Key key1 = this->key1(), key2 = this->key2(); + const Key key1 = this->template key<1>(), key2 = this->template key<2>(); JacobianC H1; JacobianL H2; Vector2 b; @@ -184,6 +184,7 @@ class GeneralSFMFactor: public NoiseModelFactor2 { friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int /*version*/) { + // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); @@ -200,7 +201,7 @@ struct traits > : Testable< * Compared to GeneralSFMFactor, it is a ternary-factor because the calibration is isolated from camera.. */ template -class GeneralSFMFactor2: public NoiseModelFactor3 { +class GeneralSFMFactor2: public NoiseModelFactorN { GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) static const int DimK = FixedDimension::value; @@ -213,7 +214,7 @@ class GeneralSFMFactor2: public NoiseModelFactor3 { typedef GeneralSFMFactor2 This; typedef PinholeCamera Camera;///< typedef for camera type - typedef NoiseModelFactor3 Base;///< typedef for the base class + typedef NoiseModelFactorN Base;///< typedef for the base class // shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -269,8 +270,8 @@ class GeneralSFMFactor2: public NoiseModelFactor3 { if (H1) *H1 = Matrix::Zero(2, 6); if (H2) *H2 = Matrix::Zero(2, 3); if (H3) *H3 = Matrix::Zero(2, DimK); - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) - << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl; + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) + << " behind Camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl; } return Z_2x1; } @@ -285,6 +286,7 @@ class GeneralSFMFactor2: public NoiseModelFactor3 { friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int /*version*/) { + // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor3", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 43404df537..6bb1b0f360 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -228,7 +228,7 @@ void InitializePose3::createSymbolicGraph( Rot3 Rij = pose3Between->measured().rotation(); factorId2RotMap->emplace(factorId, Rij); - Key key1 = pose3Between->key1(); + Key key1 = pose3Between->key<1>(); if (adjEdgesMap->find(key1) != adjEdgesMap->end()) { // key is already in adjEdgesMap->at(key1).push_back(factorId); } else { @@ -236,7 +236,7 @@ void InitializePose3::createSymbolicGraph( edge_id.push_back(factorId); adjEdgesMap->emplace(key1, edge_id); } - Key key2 = pose3Between->key2(); + Key key2 = pose3Between->key<2>(); if (adjEdgesMap->find(key2) != adjEdgesMap->end()) { // key is already in adjEdgesMap->at(key2).push_back(factorId); } else { diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index d7508f6b8a..7ead4ad115 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -15,8 +15,8 @@ namespace gtsam { void OrientedPlane3Factor::print(const string& s, const KeyFormatter& keyFormatter) const { cout << s << (s == "" ? "" : "\n"); - cout << "OrientedPlane3Factor Factor (" << keyFormatter(key1()) << ", " - << keyFormatter(key2()) << ")\n"; + cout << "OrientedPlane3Factor Factor (" << keyFormatter(key<1>()) << ", " + << keyFormatter(key<2>()) << ")\n"; measured_p_.print("Measured Plane"); this->noiseModel_->print(" noise model: "); } diff --git a/gtsam/slam/OrientedPlane3Factor.h b/gtsam/slam/OrientedPlane3Factor.h index 81bb790de7..1550201ecd 100644 --- a/gtsam/slam/OrientedPlane3Factor.h +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -15,10 +15,10 @@ namespace gtsam { /** * Factor to measure a planar landmark from a given pose */ -class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactor2 { +class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactorN { protected: OrientedPlane3 measured_p_; - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; public: /// Constructor @@ -49,10 +49,10 @@ class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactor2 { +class GTSAM_EXPORT OrientedPlane3DirectionPrior : public NoiseModelFactorN { protected: OrientedPlane3 measured_p_; /// measured plane parameters - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; public: typedef OrientedPlane3DirectionPrior This; diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h index 20f12dbce2..759e66341d 100644 --- a/gtsam/slam/PoseRotationPrior.h +++ b/gtsam/slam/PoseRotationPrior.h @@ -16,11 +16,11 @@ namespace gtsam { template -class PoseRotationPrior : public NoiseModelFactor1 { +class PoseRotationPrior : public NoiseModelFactorN { public: typedef PoseRotationPrior This; - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; typedef POSE Pose; typedef typename POSE::Translation Translation; typedef typename POSE::Rotation Rotation; @@ -92,6 +92,7 @@ class PoseRotationPrior : public NoiseModelFactor1 { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h index 5bb3745e9b..e009ace293 100644 --- a/gtsam/slam/PoseTranslationPrior.h +++ b/gtsam/slam/PoseTranslationPrior.h @@ -18,10 +18,10 @@ namespace gtsam { * A prior on the translation part of a pose */ template -class PoseTranslationPrior : public NoiseModelFactor1 { +class PoseTranslationPrior : public NoiseModelFactorN { public: typedef PoseTranslationPrior This; - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; typedef POSE Pose; typedef typename POSE::Translation Translation; typedef typename POSE::Rotation Rotation; @@ -91,6 +91,7 @@ class PoseTranslationPrior : public NoiseModelFactor1 { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 0fc1c0b54b..add6c86c4c 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -37,7 +37,7 @@ namespace gtsam { */ template - class GenericProjectionFactor: public NoiseModelFactor2 { + class GenericProjectionFactor: public NoiseModelFactorN { protected: // Keep a copy of measurement and calibration for I/O @@ -52,7 +52,7 @@ namespace gtsam { public: /// shorthand for base class type - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; /// shorthand for this class typedef GenericProjectionFactor This; @@ -154,10 +154,10 @@ namespace gtsam { if (H1) *H1 = Matrix::Zero(2,6); if (H2) *H2 = Matrix::Zero(2,3); if (verboseCheirality_) - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << - " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) << + " moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl; if (throwCheirality_) - throw CheiralityException(this->key2()); + throw CheiralityException(this->template key<2>()); } return Vector2::Constant(2.0 * K_->fx()); } diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h index 40c8e29b74..74e8444046 100644 --- a/gtsam/slam/ReferenceFrameFactor.h +++ b/gtsam/slam/ReferenceFrameFactor.h @@ -54,13 +54,13 @@ P transform_point( * specific classes of landmarks */ template -class ReferenceFrameFactor : public NoiseModelFactor3 { +class ReferenceFrameFactor : public NoiseModelFactorN { protected: /** default constructor for serialization only */ ReferenceFrameFactor() {} public: - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; typedef ReferenceFrameFactor This; typedef POINT Point; @@ -107,16 +107,16 @@ class ReferenceFrameFactor : public NoiseModelFactor3 { void print(const std::string& s="", const gtsam::KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << ": ReferenceFrameFactor(" - << "Global: " << keyFormatter(this->key1()) << "," - << " Transform: " << keyFormatter(this->key2()) << "," - << " Local: " << keyFormatter(this->key3()) << ")\n"; + << "Global: " << keyFormatter(this->template key<1>()) << "," + << " Transform: " << keyFormatter(this->template key<2>()) << "," + << " Local: " << keyFormatter(this->template key<3>()) << ")\n"; this->noiseModel_->print(" noise model"); } // access - convenience functions - Key global_key() const { return this->key1(); } - Key transform_key() const { return this->key2(); } - Key local_key() const { return this->key3(); } + Key global_key() const { return this->template key<1>(); } + Key transform_key() const { return this->template key<2>(); } + Key local_key() const { return this->template key<3>(); } private: /** Serialization function */ diff --git a/gtsam/slam/RotateFactor.h b/gtsam/slam/RotateFactor.h index 9e4091111c..85539e17e5 100644 --- a/gtsam/slam/RotateFactor.h +++ b/gtsam/slam/RotateFactor.h @@ -20,11 +20,11 @@ namespace gtsam { * with z and p measured and predicted angular velocities, and hence * p = iRc * z */ -class RotateFactor: public NoiseModelFactor1 { +class RotateFactor: public NoiseModelFactorN { Point3 p_, z_; ///< Predicted and measured directions, p = iRc * z - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; typedef RotateFactor This; public: @@ -64,11 +64,11 @@ class RotateFactor: public NoiseModelFactor1 { * Factor on unknown rotation iRc that relates two directions c * Directions provide less constraints than a full rotation */ -class RotateDirectionsFactor: public NoiseModelFactor1 { +class RotateDirectionsFactor: public NoiseModelFactorN { Unit3 i_p_, c_z_; ///< Predicted and measured directions, i_p = iRc * c_z - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; typedef RotateDirectionsFactor This; public: diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index ca1774e310..3be255e45c 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -28,7 +28,7 @@ namespace gtsam { * @ingroup slam */ template -class GenericStereoFactor: public NoiseModelFactor2 { +class GenericStereoFactor: public NoiseModelFactorN { private: // Keep a copy of measurement and calibration for I/O @@ -43,7 +43,7 @@ class GenericStereoFactor: public NoiseModelFactor2 { public: // shorthand for base class type - typedef NoiseModelFactor2 Base; ///< typedef for base class + typedef NoiseModelFactorN Base; ///< typedef for base class typedef GenericStereoFactor This; ///< typedef for this class (with templates) typedef boost::shared_ptr shared_ptr; ///< typedef for shared pointer to this object typedef POSE CamPose; ///< typedef for Pose Lie Value type @@ -141,8 +141,8 @@ class GenericStereoFactor: public NoiseModelFactor2 { if (H1) *H1 = Matrix::Zero(3,6); if (H2) *H2 = Z_3x3; if (verboseCheirality_) - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << - " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) << + " moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl; if (throwCheirality_) throw StereoCheiralityException(this->key2()); } @@ -170,6 +170,7 @@ class GenericStereoFactor: public NoiseModelFactor2 { friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int /*version*/) { + // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index 80d3230199..f33ba2ca22 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -30,7 +30,7 @@ namespace gtsam { * @ingroup slam */ template -class TriangulationFactor: public NoiseModelFactor1 { +class TriangulationFactor: public NoiseModelFactorN { public: @@ -40,7 +40,7 @@ class TriangulationFactor: public NoiseModelFactor1 { protected: /// shorthand for base class type - using Base = NoiseModelFactor1; + using Base = NoiseModelFactorN; /// shorthand for this class using This = TriangulationFactor; diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 511cbd8390..f25d59ab7c 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -535,7 +535,7 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, graph->push_back(*f); // Insert vertices if pure odometry file - Key key1 = (*f)->key1(), key2 = (*f)->key2(); + Key key1 = (*f)->key<1>(), key2 = (*f)->key<2>(); if (!initial->exists(key1)) initial->insert(key1, Pose2()); if (!initial->exists(key2)) @@ -603,7 +603,7 @@ void save2D(const NonlinearFactorGraph &graph, const Values &config, continue; const Pose2 pose = factor->measured().inverse(); - stream << "EDGE2 " << factor->key2() << " " << factor->key1() << " " + stream << "EDGE2 " << factor->key<2>() << " " << factor->key<1>() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << " " << RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2) << " " << RR(0, 2) << " " << RR(1, 2) << endl; @@ -691,8 +691,8 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, } Matrix3 Info = gaussianModel->R().transpose() * gaussianModel->R(); Pose2 pose = factor->measured(); //.inverse(); - stream << "EDGE_SE2 " << index(factor->key1()) << " " - << index(factor->key2()) << " " << pose.x() << " " << pose.y() + stream << "EDGE_SE2 " << index(factor->key<1>()) << " " + << index(factor->key<2>()) << " " << pose.x() << " " << pose.y() << " " << pose.theta(); for (size_t i = 0; i < 3; i++) { for (size_t j = i; j < 3; j++) { @@ -717,8 +717,8 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, const Pose3 pose3D = factor3D->measured(); const Point3 p = pose3D.translation(); const auto q = pose3D.rotation().toQuaternion(); - stream << "EDGE_SE3:QUAT " << index(factor3D->key1()) << " " - << index(factor3D->key2()) << " " << p.x() << " " << p.y() << " " + stream << "EDGE_SE3:QUAT " << index(factor3D->key<1>()) << " " + << index(factor3D->key<2>()) << " " << p.x() << " " << p.y() << " " << p.z() << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w(); diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h index 9f00f81d66..dcca226955 100644 --- a/gtsam_unstable/dynamics/FullIMUFactor.h +++ b/gtsam_unstable/dynamics/FullIMUFactor.h @@ -22,9 +22,9 @@ namespace gtsam { * assumed to be PoseRTV */ template -class FullIMUFactor : public NoiseModelFactor2 { +class FullIMUFactor : public NoiseModelFactorN { public: - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; typedef FullIMUFactor This; protected: diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h index 9a742b4f0e..4eaa3139da 100644 --- a/gtsam_unstable/dynamics/IMUFactor.h +++ b/gtsam_unstable/dynamics/IMUFactor.h @@ -20,9 +20,9 @@ namespace gtsam { * assumed to be PoseRTV */ template -class IMUFactor : public NoiseModelFactor2 { +class IMUFactor : public NoiseModelFactorN { public: - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; typedef IMUFactor This; protected: diff --git a/gtsam_unstable/dynamics/Pendulum.h b/gtsam_unstable/dynamics/Pendulum.h index 1e0ca621cd..d7301a5767 100644 --- a/gtsam_unstable/dynamics/Pendulum.h +++ b/gtsam_unstable/dynamics/Pendulum.h @@ -20,11 +20,11 @@ namespace gtsam { * - For implicit Euler method: q_{k+1} = q_k + h*v_{k+1} * - For sympletic Euler method: q_{k+1} = q_k + h*v_{k+1} */ -class PendulumFactor1: public NoiseModelFactor3 { +class PendulumFactor1: public NoiseModelFactorN { public: protected: - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; /** default constructor to allow for serialization */ PendulumFactor1() {} @@ -66,11 +66,11 @@ class PendulumFactor1: public NoiseModelFactor3 { * - For implicit Euler method: v_{k+1} = v_k - h*g/L*sin(q_{k+1}) * - For sympletic Euler method: v_{k+1} = v_k - h*g/L*sin(q_k) */ -class PendulumFactor2: public NoiseModelFactor3 { +class PendulumFactor2: public NoiseModelFactorN { public: protected: - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; /** default constructor to allow for serialization */ PendulumFactor2() {} @@ -113,11 +113,11 @@ class PendulumFactor2: public NoiseModelFactor3 { * \f$ p_k = -D_1 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)+mgrh(1-\alpha)\,\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right) \f$ * \f$ = (1/h)mr^2 (q_{k+1}-q_k) + mgrh(1-alpha) sin ((1-alpha)q_k+\alpha q_{k+1}) \f$ */ -class PendulumFactorPk: public NoiseModelFactor3 { +class PendulumFactorPk: public NoiseModelFactorN { public: protected: - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; /** default constructor to allow for serialization */ PendulumFactorPk() {} @@ -169,11 +169,11 @@ class PendulumFactorPk: public NoiseModelFactor3 { * \f$ p_k1 = D_2 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)-mgrh\alpha\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right) \f$ * \f$ = (1/h)mr^2 (q_{k+1}-q_k) - mgrh alpha sin ((1-alpha)q_k+\alpha q_{k+1}) \f$ */ -class PendulumFactorPk1: public NoiseModelFactor3 { +class PendulumFactorPk1: public NoiseModelFactorN { public: protected: - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; /** default constructor to allow for serialization */ PendulumFactorPk1() {} diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h index 1fb8958337..941b7c7acc 100644 --- a/gtsam_unstable/dynamics/SimpleHelicopter.h +++ b/gtsam_unstable/dynamics/SimpleHelicopter.h @@ -24,10 +24,10 @@ namespace gtsam { * Note: this factor is necessary if one needs to smooth the entire graph. It's not needed * in sequential update method. */ -class Reconstruction : public NoiseModelFactor3 { +class Reconstruction : public NoiseModelFactorN { double h_; // time step - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; public: Reconstruction(Key gKey1, Key gKey, Key xiKey, double h, double mu = 1000.0) : Base(noiseModel::Constrained::All(6, std::abs(mu)), gKey1, gKey, @@ -73,7 +73,7 @@ class Reconstruction : public NoiseModelFactor3 { /** * Implement the Discrete Euler-Poincare' equation: */ -class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3 { +class DiscreteEulerPoincareHelicopter : public NoiseModelFactorN { double h_; /// time step Matrix Inertia_; /// Inertia tensors Inertia = [ J 0; 0 M ] @@ -86,7 +86,7 @@ class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; public: DiscreteEulerPoincareHelicopter(Key xiKey1, Key xiKey_1, Key gKey, diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index d24d06e798..0fa3d9cb73 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -32,9 +32,9 @@ typedef enum { * NOTE: this approximation is insufficient for large timesteps, but is accurate * if timesteps are small. */ -class VelocityConstraint : public gtsam::NoiseModelFactor2 { +class VelocityConstraint : public gtsam::NoiseModelFactorN { public: - typedef gtsam::NoiseModelFactor2 Base; + typedef gtsam::NoiseModelFactorN Base; protected: diff --git a/gtsam_unstable/dynamics/VelocityConstraint3.h b/gtsam_unstable/dynamics/VelocityConstraint3.h index c9f05cf98f..2880b9c8cd 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint3.h +++ b/gtsam_unstable/dynamics/VelocityConstraint3.h @@ -10,11 +10,11 @@ namespace gtsam { -class VelocityConstraint3 : public NoiseModelFactor3 { +class VelocityConstraint3 : public NoiseModelFactorN { public: protected: - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; /** default constructor to allow for serialization */ VelocityConstraint3() {} @@ -53,6 +53,7 @@ class VelocityConstraint3 : public NoiseModelFactor3 { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor3", boost::serialization::base_object(*this)); } diff --git a/gtsam_unstable/slam/BiasedGPSFactor.h b/gtsam_unstable/slam/BiasedGPSFactor.h index 250c15b830..6f0aa605b8 100644 --- a/gtsam_unstable/slam/BiasedGPSFactor.h +++ b/gtsam_unstable/slam/BiasedGPSFactor.h @@ -27,12 +27,12 @@ namespace gtsam { * common-mode errors and that can be partially corrected if other sensors are used * @ingroup slam */ - class BiasedGPSFactor: public NoiseModelFactor2 { + class BiasedGPSFactor: public NoiseModelFactorN { private: typedef BiasedGPSFactor This; - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; Point3 measured_; /** The measurement */ @@ -57,8 +57,8 @@ namespace gtsam { /** print */ void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "BiasedGPSFactor(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << ")\n" + << keyFormatter(this->key<1>()) << "," + << keyFormatter(this->key<2>()) << ")\n" << " measured: " << measured_.transpose() << std::endl; this->noiseModel_->print(" noise model: "); } @@ -96,6 +96,7 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index cabbfdbe85..43001fbc2a 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -88,12 +88,12 @@ namespace gtsam { */ template -class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5 { +class EquivInertialNavFactor_GlobalVel : public NoiseModelFactorN { private: typedef EquivInertialNavFactor_GlobalVel This; - typedef NoiseModelFactor5 Base; + typedef NoiseModelFactorN Base; Vector delta_pos_in_t0_; Vector delta_vel_in_t0_; @@ -136,11 +136,11 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5key1()) << "," - << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," - << keyFormatter(this->key4()) << "," - << keyFormatter(this->key5()) << "\n"; + << keyFormatter(this->template key<1>()) << "," + << keyFormatter(this->template key<2>()) << "," + << keyFormatter(this->template key<3>()) << "," + << keyFormatter(this->template key<4>()) << "," + << keyFormatter(this->template key<5>()) << "\n"; std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl; std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl; std::cout << "delta_angles: " << this->delta_angles_ << std::endl; diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index b053b13f82..6423fabda3 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -87,12 +87,12 @@ namespace gtsam { */ template -class EquivInertialNavFactor_GlobalVel_NoBias : public NoiseModelFactor4 { +class EquivInertialNavFactor_GlobalVel_NoBias : public NoiseModelFactorN { private: typedef EquivInertialNavFactor_GlobalVel_NoBias This; - typedef NoiseModelFactor4 Base; + typedef NoiseModelFactorN Base; Vector delta_pos_in_t0_; Vector delta_vel_in_t0_; @@ -136,10 +136,10 @@ class EquivInertialNavFactor_GlobalVel_NoBias : public NoiseModelFactor4key1()) << "," - << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," - << keyFormatter(this->key4()) << "\n"; + << keyFormatter(this->key<1>()) << "," + << keyFormatter(this->key<2>()) << "," + << keyFormatter(this->key<3>()) << "," + << keyFormatter(this->key<4>()) << "\n"; std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl; std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl; std::cout << "delta_angles: " << this->delta_angles_ << std::endl; diff --git a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h index 52e4f44cb3..c3682e536d 100644 --- a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h +++ b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h @@ -42,12 +42,12 @@ namespace gtsam { * T is the measurement type, by default the same */ template -class GaussMarkov1stOrderFactor: public NoiseModelFactor2 { +class GaussMarkov1stOrderFactor: public NoiseModelFactorN { private: typedef GaussMarkov1stOrderFactor This; - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; double dt_; Vector tau_; @@ -73,8 +73,8 @@ class GaussMarkov1stOrderFactor: public NoiseModelFactor2 { /** print */ void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "GaussMarkov1stOrderFactor(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << ")\n"; + << keyFormatter(this->template key<1>()) << "," + << keyFormatter(this->template key<2>()) << ")\n"; this->noiseModel_->print(" noise model"); } diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 0828fbd088..efaf71d229 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -77,12 +77,12 @@ namespace gtsam { * vehicle */ template -class InertialNavFactor_GlobalVelocity : public NoiseModelFactor5 { +class InertialNavFactor_GlobalVelocity : public NoiseModelFactorN { private: typedef InertialNavFactor_GlobalVelocity This; - typedef NoiseModelFactor5 Base; + typedef NoiseModelFactorN Base; Vector measurement_acc_; Vector measurement_gyro_; @@ -117,11 +117,11 @@ class InertialNavFactor_GlobalVelocity : public NoiseModelFactor5key1()) << "," - << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," - << keyFormatter(this->key4()) << "," - << keyFormatter(this->key5()) << "\n"; + << keyFormatter(this->template key<1>()) << "," + << keyFormatter(this->template key<2>()) << "," + << keyFormatter(this->template key<3>()) << "," + << keyFormatter(this->template key<4>()) << "," + << keyFormatter(this->template key<5>()) << "\n"; std::cout << "acc measurement: " << this->measurement_acc_.transpose() << std::endl; std::cout << "gyro measurement: " << this->measurement_gyro_.transpose() << std::endl; std::cout << "dt: " << this->dt_ << std::endl; diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h index 44d3b8fd04..59a834f0b7 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -24,7 +24,7 @@ namespace gtsam { * Ternary factor representing a visual measurement that includes inverse depth */ template -class InvDepthFactor3: public NoiseModelFactor3 { +class InvDepthFactor3: public NoiseModelFactorN { protected: // Keep a copy of measurement and calibration for I/O @@ -34,7 +34,7 @@ class InvDepthFactor3: public NoiseModelFactor3 { public: /// shorthand for base class type - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; /// shorthand for this class typedef InvDepthFactor3 This; @@ -93,8 +93,8 @@ class InvDepthFactor3: public NoiseModelFactor3 { if (H1) *H1 = Matrix::Zero(2,6); if (H2) *H2 = Matrix::Zero(2,5); if (H3) *H3 = Matrix::Zero(2,1); - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << - " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) << + " moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl; return Vector::Ones(2) * 2.0 * K_->fx(); } return (Vector(1) << 0.0).finished(); diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index 40c09416c8..a41a06ccde 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -24,7 +24,7 @@ namespace gtsam { /** * Binary factor representing a visual measurement using an inverse-depth parameterization */ -class InvDepthFactorVariant1: public NoiseModelFactor2 { +class InvDepthFactorVariant1: public NoiseModelFactorN { protected: // Keep a copy of measurement and calibration for I/O @@ -34,7 +34,7 @@ class InvDepthFactorVariant1: public NoiseModelFactor2 { public: /// shorthand for base class type - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; /// shorthand for this class typedef InvDepthFactorVariant1 This; @@ -93,8 +93,8 @@ class InvDepthFactorVariant1: public NoiseModelFactor2 { return camera.project(world_P_landmark) - measured_; } catch( CheiralityException& e) { std::cout << e.what() - << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]" - << " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]" + << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key<2>()) << "]" + << " moved behind camera [" << DefaultKeyFormatter(this->key<1>()) <<"]" << std::endl; return Vector::Ones(2) * 2.0 * K_->fx(); } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index b1169580e3..d120eff321 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -25,7 +25,7 @@ namespace gtsam { /** * Binary factor representing a visual measurement using an inverse-depth parameterization */ -class InvDepthFactorVariant2: public NoiseModelFactor2 { +class InvDepthFactorVariant2: public NoiseModelFactorN { protected: // Keep a copy of measurement and calibration for I/O @@ -36,7 +36,7 @@ class InvDepthFactorVariant2: public NoiseModelFactor2 { public: /// shorthand for base class type - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; /// shorthand for this class typedef InvDepthFactorVariant2 This; @@ -96,8 +96,8 @@ class InvDepthFactorVariant2: public NoiseModelFactor2 { return camera.project(world_P_landmark) - measured_; } catch( CheiralityException& e) { std::cout << e.what() - << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]" - << " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]" + << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key<2>()) << "]" + << " moved behind camera [" << DefaultKeyFormatter(this->key<1>()) <<"]" << std::endl; return Vector::Ones(2) * 2.0 * K_->fx(); } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index 98f2db2f37..cade280f0d 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -24,7 +24,7 @@ namespace gtsam { /** * Binary factor representing the first visual measurement using an inverse-depth parameterization */ -class InvDepthFactorVariant3a: public NoiseModelFactor2 { +class InvDepthFactorVariant3a: public NoiseModelFactorN { protected: // Keep a copy of measurement and calibration for I/O @@ -34,7 +34,7 @@ class InvDepthFactorVariant3a: public NoiseModelFactor2 { public: /// shorthand for base class type - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; /// shorthand for this class typedef InvDepthFactorVariant3a This; @@ -96,8 +96,8 @@ class InvDepthFactorVariant3a: public NoiseModelFactor2 { return camera.project(world_P_landmark) - measured_; } catch( CheiralityException& e) { std::cout << e.what() - << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key2()) << "]" - << " moved behind camera [" << DefaultKeyFormatter(this->key1()) << "]" + << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key<1>()) << "," << DefaultKeyFormatter(this->key<2>()) << "]" + << " moved behind camera [" << DefaultKeyFormatter(this->key<1>()) << "]" << std::endl; return Vector::Ones(2) * 2.0 * K_->fx(); } @@ -150,7 +150,7 @@ class InvDepthFactorVariant3a: public NoiseModelFactor2 { /** * Ternary factor representing a visual measurement using an inverse-depth parameterization */ -class InvDepthFactorVariant3b: public NoiseModelFactor3 { +class InvDepthFactorVariant3b: public NoiseModelFactorN { protected: // Keep a copy of measurement and calibration for I/O @@ -160,7 +160,7 @@ class InvDepthFactorVariant3b: public NoiseModelFactor3 { public: /// shorthand for base class type - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; /// shorthand for this class typedef InvDepthFactorVariant3b This; @@ -222,8 +222,8 @@ class InvDepthFactorVariant3b: public NoiseModelFactor3 { return camera.project(world_P_landmark) - measured_; } catch( CheiralityException& e) { std::cout << e.what() - << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key3()) << "]" - << " moved behind camera " << DefaultKeyFormatter(this->key2()) + << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key<1>()) << "," << DefaultKeyFormatter(this->key<3>()) << "]" + << " moved behind camera " << DefaultKeyFormatter(this->key<2>()) << std::endl; return Vector::Ones(2) * 2.0 * K_->fx(); } diff --git a/gtsam_unstable/slam/LocalOrientedPlane3Factor.h b/gtsam_unstable/slam/LocalOrientedPlane3Factor.h index f81c18bfa4..f2c5dd3a97 100644 --- a/gtsam_unstable/slam/LocalOrientedPlane3Factor.h +++ b/gtsam_unstable/slam/LocalOrientedPlane3Factor.h @@ -35,10 +35,10 @@ namespace gtsam { * optimized in x1 frame in the optimization. */ class GTSAM_UNSTABLE_EXPORT LocalOrientedPlane3Factor - : public NoiseModelFactor3 { + : public NoiseModelFactorN { protected: OrientedPlane3 measured_p_; - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; public: /// Constructor LocalOrientedPlane3Factor() {} diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h index 91d79f2089..34ab51d15b 100644 --- a/gtsam_unstable/slam/MultiProjectionFactor.h +++ b/gtsam_unstable/slam/MultiProjectionFactor.h @@ -187,8 +187,8 @@ namespace gtsam { if (H1) *H1 = Matrix::Zero(2,6); if (H2) *H2 = Matrix::Zero(2,3); if (verboseCheirality_) - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << - " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->keys_.at(1)) << + " moved behind camera " << DefaultKeyFormatter(this->keys_.at(0)) << std::endl; if (throwCheirality_) throw e; } diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index 52a57b945c..b49b491314 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -35,7 +35,7 @@ namespace gtsam { * @tparam VALUE is the type of variable the prior effects */ template - class PartialPriorFactor: public NoiseModelFactor1 { + class PartialPriorFactor: public NoiseModelFactorN { public: typedef VALUE T; @@ -44,7 +44,7 @@ namespace gtsam { // Concept checks on the variable type - currently requires Lie GTSAM_CONCEPT_LIE_TYPE(VALUE) - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; typedef PartialPriorFactor This; Vector prior_; ///< Measurement on tangent space parameters, in compressed form. @@ -141,6 +141,7 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(prior_); diff --git a/gtsam_unstable/slam/PoseBetweenFactor.h b/gtsam_unstable/slam/PoseBetweenFactor.h index b47dcaf33b..676e011de2 100644 --- a/gtsam_unstable/slam/PoseBetweenFactor.h +++ b/gtsam_unstable/slam/PoseBetweenFactor.h @@ -29,12 +29,12 @@ namespace gtsam { * @ingroup slam */ template - class PoseBetweenFactor: public NoiseModelFactor2 { + class PoseBetweenFactor: public NoiseModelFactorN { private: typedef PoseBetweenFactor This; - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; POSE measured_; /** The measurement */ boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame @@ -68,8 +68,8 @@ namespace gtsam { /** print */ void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "BetweenFactor(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << ")\n"; + << keyFormatter(this->template key<1>()) << "," + << keyFormatter(this->template key<2>()) << ")\n"; measured_.print(" measured: "); if(this->body_P_sensor_) this->body_P_sensor_->print(" sensor pose in body frame: "); diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h index b0cb25cfaf..c7a094bda3 100644 --- a/gtsam_unstable/slam/PosePriorFactor.h +++ b/gtsam_unstable/slam/PosePriorFactor.h @@ -26,12 +26,12 @@ namespace gtsam { * @ingroup slam */ template - class PosePriorFactor: public NoiseModelFactor1 { + class PosePriorFactor: public NoiseModelFactorN { private: typedef PosePriorFactor This; - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; POSE prior_; /** The measurement */ boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame diff --git a/gtsam_unstable/slam/PoseToPointFactor.h b/gtsam_unstable/slam/PoseToPointFactor.h index 8d183015e0..ad1ba5fbe6 100644 --- a/gtsam_unstable/slam/PoseToPointFactor.h +++ b/gtsam_unstable/slam/PoseToPointFactor.h @@ -21,10 +21,10 @@ namespace gtsam { * @ingroup slam */ template -class PoseToPointFactor : public NoiseModelFactor2 { +class PoseToPointFactor : public NoiseModelFactorN { private: typedef PoseToPointFactor This; - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; POINT measured_; /** the point measurement in local coordinates */ @@ -47,8 +47,9 @@ class PoseToPointFactor : public NoiseModelFactor2 { /** print */ void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { - std::cout << s << "PoseToPointFactor(" << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << ")\n" + std::cout << s << "PoseToPointFactor(" + << keyFormatter(this->template key<1>()) << "," + << keyFormatter(this->template key<2>()) << ")\n" << " measured: " << measured_.transpose() << std::endl; this->noiseModel_->print(" noise model: "); } @@ -91,8 +92,10 @@ class PoseToPointFactor : public NoiseModelFactor2 { friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility ar& boost::serialization::make_nvp( - "NoiseModelFactor2", boost::serialization::base_object(*this)); + "NoiseModelFactor2", + boost::serialization::base_object(*this)); ar& BOOST_SERIALIZATION_NVP(measured_); } diff --git a/gtsam_unstable/slam/ProjectionFactorPPP.h b/gtsam_unstable/slam/ProjectionFactorPPP.h index d6f643c6cc..8962b31b2d 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPP.h +++ b/gtsam_unstable/slam/ProjectionFactorPPP.h @@ -31,7 +31,7 @@ namespace gtsam { * @ingroup slam */ template - class ProjectionFactorPPP: public NoiseModelFactor3 { + class ProjectionFactorPPP: public NoiseModelFactorN { protected: // Keep a copy of measurement and calibration for I/O @@ -45,7 +45,7 @@ namespace gtsam { public: /// shorthand for base class type - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; /// shorthand for this class typedef ProjectionFactorPPP This; @@ -142,8 +142,11 @@ namespace gtsam { if (H2) *H2 = Matrix::Zero(2,6); if (H3) *H3 = Matrix::Zero(2,3); if (verboseCheirality_) - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << - " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; + std::cout << e.what() << ": Landmark " + << DefaultKeyFormatter(this->template key<2>()) + << " moved behind camera " + << DefaultKeyFormatter(this->template key<1>()) + << std::endl; if (throwCheirality_) throw e; } diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h index 5c9a8c6914..afbf858382 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPPC.h +++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h @@ -34,7 +34,7 @@ namespace gtsam { */ template class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC - : public NoiseModelFactor4 { + : public NoiseModelFactorN { protected: Point2 measured_; ///< 2D measurement @@ -44,7 +44,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC public: /// shorthand for base class type - typedef NoiseModelFactor4 Base; + typedef NoiseModelFactorN Base; /// shorthand for this class typedef ProjectionFactorPPPC This; @@ -130,8 +130,8 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC if (H3) *H3 = Matrix::Zero(2,3); if (H4) *H4 = Matrix::Zero(2,CALIBRATION::Dim()); if (verboseCheirality_) - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << - " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->template key<2>()) << + " moved behind camera " << DefaultKeyFormatter(this->template key<1>()) << std::endl; if (throwCheirality_) throw e; } diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp b/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp index c92a13daf5..8173c9dbde 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp @@ -54,9 +54,9 @@ Vector ProjectionFactorRollingShutter::evaluateError( if (H3) *H3 = Matrix::Zero(2, 3); if (verboseCheirality_) std::cout << e.what() << ": Landmark " - << DefaultKeyFormatter(this->key2()) << " moved behind camera " - << DefaultKeyFormatter(this->key1()) << std::endl; - if (throwCheirality_) throw CheiralityException(this->key2()); + << DefaultKeyFormatter(this->key<2>()) << " moved behind camera " + << DefaultKeyFormatter(this->key<1>()) << std::endl; + if (throwCheirality_) throw CheiralityException(this->key<2>()); } return Vector2::Constant(2.0 * K_->fx()); } diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index 597d894da8..806f54fa55 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -42,7 +42,7 @@ namespace gtsam { */ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter - : public NoiseModelFactor3 { + : public NoiseModelFactorN { protected: // Keep a copy of measurement and calibration for I/O Point2 measured_; ///< 2D measurement @@ -60,7 +60,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter public: /// shorthand for base class type - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; /// shorthand for this class typedef ProjectionFactorRollingShutter This; diff --git a/gtsam_unstable/slam/RelativeElevationFactor.h b/gtsam_unstable/slam/RelativeElevationFactor.h index ebf91d1f7b..c6273ff4bb 100644 --- a/gtsam_unstable/slam/RelativeElevationFactor.h +++ b/gtsam_unstable/slam/RelativeElevationFactor.h @@ -25,13 +25,13 @@ namespace gtsam { * * TODO: enable use of a Pose3 for the target as well */ -class GTSAM_UNSTABLE_EXPORT RelativeElevationFactor: public NoiseModelFactor2 { +class GTSAM_UNSTABLE_EXPORT RelativeElevationFactor: public NoiseModelFactorN { private: double measured_; /** measurement */ typedef RelativeElevationFactor This; - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; public: @@ -66,6 +66,7 @@ class GTSAM_UNSTABLE_EXPORT RelativeElevationFactor: public NoiseModelFactor2 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); diff --git a/gtsam_unstable/slam/TSAMFactors.h b/gtsam_unstable/slam/TSAMFactors.h index 6f98a2dbdc..2e024c5bbe 100644 --- a/gtsam_unstable/slam/TSAMFactors.h +++ b/gtsam_unstable/slam/TSAMFactors.h @@ -26,11 +26,11 @@ namespace gtsam { /** * DeltaFactor: relative 2D measurement between Pose2 and Point2 */ -class DeltaFactor: public NoiseModelFactor2 { +class DeltaFactor: public NoiseModelFactorN { public: typedef DeltaFactor This; - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; typedef boost::shared_ptr shared_ptr; private: @@ -55,11 +55,11 @@ class DeltaFactor: public NoiseModelFactor2 { /** * DeltaFactorBase: relative 2D measurement between Pose2 and Point2, with Basenodes */ -class DeltaFactorBase: public NoiseModelFactor4 { +class DeltaFactorBase: public NoiseModelFactorN { public: typedef DeltaFactorBase This; - typedef NoiseModelFactor4 Base; + typedef NoiseModelFactorN Base; typedef boost::shared_ptr shared_ptr; private: @@ -110,11 +110,11 @@ class DeltaFactorBase: public NoiseModelFactor4 { /** * OdometryFactorBase: Pose2 odometry, with Basenodes */ -class OdometryFactorBase: public NoiseModelFactor4 { +class OdometryFactorBase: public NoiseModelFactorN { public: typedef OdometryFactorBase This; - typedef NoiseModelFactor4 Base; + typedef NoiseModelFactorN Base; typedef boost::shared_ptr shared_ptr; private: diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h index 3afa83f45b..afe731bd5f 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h @@ -32,7 +32,7 @@ namespace gtsam { * @ingroup slam */ template - class TransformBtwRobotsUnaryFactor: public NonlinearFactor { // TODO why not NoiseModelFactor1 ? + class TransformBtwRobotsUnaryFactor: public NonlinearFactor { // TODO why not NoiseModelFactorN ? public: diff --git a/tests/simulated2D.h b/tests/simulated2D.h index 097dbd3feb..2cd0d0f392 100644 --- a/tests/simulated2D.h +++ b/tests/simulated2D.h @@ -124,9 +124,9 @@ namespace simulated2D { * Unary factor encoding a soft prior on a vector */ template - class GenericPrior: public NoiseModelFactor1 { + class GenericPrior: public NoiseModelFactorN { public: - typedef NoiseModelFactor1 Base; ///< base class + typedef NoiseModelFactorN Base; ///< base class typedef GenericPrior This; typedef boost::shared_ptr > shared_ptr; typedef VALUE Pose; ///< shortcut to Pose type @@ -168,9 +168,9 @@ namespace simulated2D { * Binary factor simulating "odometry" between two Vectors */ template - class GenericOdometry: public NoiseModelFactor2 { + class GenericOdometry: public NoiseModelFactorN { public: - typedef NoiseModelFactor2 Base; ///< base class + typedef NoiseModelFactorN Base; ///< base class typedef GenericOdometry This; typedef boost::shared_ptr > shared_ptr; typedef VALUE Pose; ///< shortcut to Pose type @@ -214,9 +214,9 @@ namespace simulated2D { * Binary factor simulating "measurement" between two Vectors */ template - class GenericMeasurement: public NoiseModelFactor2 { + class GenericMeasurement: public NoiseModelFactorN { public: - typedef NoiseModelFactor2 Base; ///< base class + typedef NoiseModelFactorN Base; ///< base class typedef GenericMeasurement This; typedef boost::shared_ptr > shared_ptr; typedef POSE Pose; ///< shortcut to Pose type diff --git a/tests/simulated2DOriented.h b/tests/simulated2DOriented.h index 924a5fe4e1..086da7cad6 100644 --- a/tests/simulated2DOriented.h +++ b/tests/simulated2DOriented.h @@ -80,13 +80,13 @@ namespace simulated2DOriented { /// Unary factor encoding a soft prior on a vector template - struct GenericPosePrior: public NoiseModelFactor1 { + struct GenericPosePrior: public NoiseModelFactorN { Pose2 measured_; ///< measurement /// Create generic pose prior GenericPosePrior(const Pose2& measured, const SharedNoiseModel& model, Key key) : - NoiseModelFactor1(model, key), measured_(measured) { + NoiseModelFactorN(model, key), measured_(measured) { } /// Evaluate error and optionally derivative @@ -101,7 +101,7 @@ namespace simulated2DOriented { * Binary factor simulating "odometry" between two Vectors */ template - struct GenericOdometry: public NoiseModelFactor2 { + struct GenericOdometry: public NoiseModelFactorN { Pose2 measured_; ///< Between measurement for odometry factor typedef GenericOdometry This; @@ -111,7 +111,7 @@ namespace simulated2DOriented { */ GenericOdometry(const Pose2& measured, const SharedNoiseModel& model, Key i1, Key i2) : - NoiseModelFactor2(model, i1, i2), measured_(measured) { + NoiseModelFactorN(model, i1, i2), measured_(measured) { } ~GenericOdometry() override {} diff --git a/tests/simulated3D.h b/tests/simulated3D.h index 3b4afb1063..4a20acd159 100644 --- a/tests/simulated3D.h +++ b/tests/simulated3D.h @@ -68,7 +68,7 @@ Point3 mea(const Point3& x, const Point3& l, /** * A prior factor on a single linear robot pose */ -struct PointPrior3D: public NoiseModelFactor1 { +struct PointPrior3D: public NoiseModelFactorN { Point3 measured_; ///< The prior pose value for the variable attached to this factor @@ -79,7 +79,7 @@ struct PointPrior3D: public NoiseModelFactor1 { * @param key is the key for the pose */ PointPrior3D(const Point3& measured, const SharedNoiseModel& model, Key key) : - NoiseModelFactor1 (model, key), measured_(measured) { + NoiseModelFactorN (model, key), measured_(measured) { } /** @@ -98,7 +98,7 @@ struct PointPrior3D: public NoiseModelFactor1 { /** * Models a linear 3D measurement between 3D points */ -struct Simulated3DMeasurement: public NoiseModelFactor2 { +struct Simulated3DMeasurement: public NoiseModelFactorN { Point3 measured_; ///< Linear displacement between a pose and landmark @@ -110,7 +110,7 @@ struct Simulated3DMeasurement: public NoiseModelFactor2 { * @param pointKey is the point key for the landmark */ Simulated3DMeasurement(const Point3& measured, const SharedNoiseModel& model, Key i, Key j) : - NoiseModelFactor2(model, i, j), measured_(measured) {} + NoiseModelFactorN(model, i, j), measured_(measured) {} /** * Error function with optional derivatives diff --git a/tests/smallExample.h b/tests/smallExample.h index ca9a8580fc..38b202c418 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -329,12 +329,12 @@ inline Matrix H(const Point2& v) { 0.0, cos(v.y())).finished(); } -struct UnaryFactor: public gtsam::NoiseModelFactor1 { +struct UnaryFactor: public gtsam::NoiseModelFactorN { Point2 z_; UnaryFactor(const Point2& z, const SharedNoiseModel& model, Key key) : - gtsam::NoiseModelFactor1(model, key), z_(z) { + gtsam::NoiseModelFactorN(model, key), z_(z) { } Vector evaluateError(const Point2& x, boost::optional A = boost::none) const override { diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index f0c1b4b703..88da7006d1 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -91,9 +91,9 @@ TEST( ExtendedKalmanFilter, linear ) { // Create Motion Model Factor -class NonlinearMotionModel : public NoiseModelFactor2 { +class NonlinearMotionModel : public NoiseModelFactorN { - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; typedef NonlinearMotionModel This; protected: @@ -155,14 +155,14 @@ class NonlinearMotionModel : public NoiseModelFactor2 { /* print */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << ": NonlinearMotionModel\n"; - std::cout << " TestKey1: " << keyFormatter(key1()) << std::endl; - std::cout << " TestKey2: " << keyFormatter(key2()) << std::endl; + std::cout << " TestKey1: " << keyFormatter(key<1>()) << std::endl; + std::cout << " TestKey2: " << keyFormatter(key<2>()) << std::endl; } /** Check if two factors are equal. Note type is IndexFactor and needs cast. */ bool equals(const NonlinearFactor& f, double tol = 1e-9) const override { const This *e = dynamic_cast (&f); - return (e != nullptr) && (key1() == e->key1()) && (key2() == e->key2()); + return (e != nullptr) && (key<1>() == e->key<1>()) && (key<2>() == e->key<2>()); } /** @@ -181,7 +181,7 @@ class NonlinearMotionModel : public NoiseModelFactor2 { /** Vector of errors, whitened ! */ Vector whitenedError(const Values& c) const { - return QInvSqrt(c.at(key1()))*unwhitenedError(c); + return QInvSqrt(c.at(key<1>()))*unwhitenedError(c); } /** @@ -190,14 +190,16 @@ class NonlinearMotionModel : public NoiseModelFactor2 { * Hence b = z - h(x1,x2) = - error_vector(x) */ boost::shared_ptr linearize(const Values& c) const override { - const X1& x1 = c.at(key1()); - const X2& x2 = c.at(key2()); + using X1 = ValueType<1>; + using X2 = ValueType<2>; + const X1& x1 = c.at(key<1>()); + const X2& x2 = c.at(key<2>()); Matrix A1, A2; Vector b = -evaluateError(x1, x2, A1, A2); SharedDiagonal constrained = boost::dynamic_pointer_cast(this->noiseModel_); if (constrained.get() != nullptr) { - return JacobianFactor::shared_ptr(new JacobianFactor(key1(), A1, key2(), + return JacobianFactor::shared_ptr(new JacobianFactor(key<1>(), A1, key<2>(), A2, b, constrained)); } // "Whiten" the system before converting to a Gaussian Factor @@ -205,7 +207,7 @@ class NonlinearMotionModel : public NoiseModelFactor2 { A1 = Qinvsqrt*A1; A2 = Qinvsqrt*A2; b = Qinvsqrt*b; - return GaussianFactor::shared_ptr(new JacobianFactor(key1(), A1, key2(), + return GaussianFactor::shared_ptr(new JacobianFactor(key<1>(), A1, key<2>(), A2, b, noiseModel::Unit::Create(b.size()))); } @@ -232,9 +234,9 @@ class NonlinearMotionModel : public NoiseModelFactor2 { }; // Create Measurement Model Factor -class NonlinearMeasurementModel : public NoiseModelFactor1 { +class NonlinearMeasurementModel : public NoiseModelFactorN { - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactorN Base; typedef NonlinearMeasurementModel This; protected: @@ -320,6 +322,7 @@ class NonlinearMeasurementModel : public NoiseModelFactor1 { * Hence b = z - h(x1) = - error_vector(x) */ boost::shared_ptr linearize(const Values& c) const override { + using X = ValueType<1>; const X& x1 = c.at(key()); Matrix A1; Vector b = -evaluateError(x1, A1); diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 99ec2f501e..c536a48c3f 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -330,6 +330,13 @@ TEST( NonlinearFactor, cloneWithNewNoiseModel ) } /* ************************************************************************* */ +// Suppress deprecation warnings while we are testing backwards compatibility +#define IGNORE_DEPRECATED_PUSH \ + CLANG_DIAGNOSTIC_PUSH_IGNORE("-Wdeprecated-declarations") \ + GCC_DIAGNOSTIC_PUSH_IGNORE("-Wdeprecated-declarations") \ + MSVC_DIAGNOSTIC_PUSH_IGNORE(4996) +/* ************************************************************************* */ +IGNORE_DEPRECATED_PUSH class TestFactor1 : public NoiseModelFactor1 { static_assert(std::is_same::value, "Base type wrong"); static_assert(std::is_same>::value, @@ -351,6 +358,7 @@ class TestFactor1 : public NoiseModelFactor1 { gtsam::NonlinearFactor::shared_ptr(new TestFactor1(*this))); } }; +DIAGNOSTIC_POP() /* ************************************ */ TEST(NonlinearFactor, NoiseModelFactor1) { @@ -380,6 +388,7 @@ TEST(NonlinearFactor, NoiseModelFactor1) { } /* ************************************************************************* */ +IGNORE_DEPRECATED_PUSH class TestFactor4 : public NoiseModelFactor4 { static_assert(std::is_same::value, "Base type wrong"); static_assert( @@ -411,6 +420,7 @@ class TestFactor4 : public NoiseModelFactor4 { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new TestFactor4(*this))); } }; +DIAGNOSTIC_POP() /* ************************************ */ TEST(NonlinearFactor, NoiseModelFactor4) { @@ -434,6 +444,7 @@ TEST(NonlinearFactor, NoiseModelFactor4) { EXPECT(assert_equal((Vector)(Vector(1) << 0.5 * -30.).finished(), jf.getb())); // Test all functions/types for backwards compatibility + IGNORE_DEPRECATED_PUSH static_assert(std::is_same::value, "X1 type incorrect"); static_assert(std::is_same::value, @@ -452,6 +463,7 @@ TEST(NonlinearFactor, NoiseModelFactor4) { EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.).finished(), H.at(1))); EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 3.).finished(), H.at(2))); EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 4.).finished(), H.at(3))); + DIAGNOSTIC_POP() // And test "forward compatibility" using `key` and `ValueType` too static_assert(std::is_same, double>::value, @@ -477,6 +489,7 @@ TEST(NonlinearFactor, NoiseModelFactor4) { } /* ************************************************************************* */ +IGNORE_DEPRECATED_PUSH class TestFactor5 : public NoiseModelFactor5 { public: typedef NoiseModelFactor5 Base; @@ -500,6 +513,7 @@ class TestFactor5 : public NoiseModelFactor5 { public: typedef NoiseModelFactor6 Base; @@ -554,6 +569,7 @@ class TestFactor6 : public NoiseModelFactor6::value, + "X1 type incorrect"); + EXPECT(assert_equal(tf.key3(), X(3))); + DIAGNOSTIC_POP() + // Test using `key` and `ValueType` static_assert(std::is_same, double>::value, "ValueType<1> type incorrect"); diff --git a/timing/timeIncremental.cpp b/timing/timeIncremental.cpp index 5e3fc91898..2b5ec474d5 100644 --- a/timing/timeIncremental.cpp +++ b/timing/timeIncremental.cpp @@ -35,8 +35,8 @@ using namespace gtsam::symbol_shorthand; typedef Pose2 Pose; -typedef NoiseModelFactor1 NM1; -typedef NoiseModelFactor2 NM2; +typedef NoiseModelFactorN NM1; +typedef NoiseModelFactorN NM2; typedef BearingRangeFactor BR; //GTSAM_VALUE_EXPORT(Value); @@ -107,18 +107,18 @@ int main(int argc, char *argv[]) { boost::dynamic_pointer_cast >(measurementf)) { // Stop collecting measurements that are for future steps - if(measurement->key1() > step || measurement->key2() > step) + if(measurement->key<1>() > step || measurement->key<2>() > step) break; // Require that one of the nodes is the current one - if(measurement->key1() != step && measurement->key2() != step) + if(measurement->key<1>() != step && measurement->key<2>() != step) throw runtime_error("Problem in data file, out-of-sequence measurements"); // Add a new factor newFactors.push_back(measurement); // Initialize the new variable - if(measurement->key1() == step && measurement->key2() == step-1) { + if(measurement->key<1>() == step && measurement->key<2>() == step-1) { if(step == 1) newVariables.insert(step, measurement->measured().inverse()); else { @@ -126,7 +126,7 @@ int main(int argc, char *argv[]) { newVariables.insert(step, prevPose * measurement->measured().inverse()); } // cout << "Initializing " << step << endl; - } else if(measurement->key2() == step && measurement->key1() == step-1) { + } else if(measurement->key<2>() == step && measurement->key<1>() == step-1) { if(step == 1) newVariables.insert(step, measurement->measured()); else { diff --git a/timing/timeiSAM2Chain.cpp b/timing/timeiSAM2Chain.cpp index a056bde246..dcb42f763e 100644 --- a/timing/timeiSAM2Chain.cpp +++ b/timing/timeiSAM2Chain.cpp @@ -36,8 +36,8 @@ using namespace gtsam::symbol_shorthand; typedef Pose2 Pose; -typedef NoiseModelFactor1 NM1; -typedef NoiseModelFactor2 NM2; +typedef NoiseModelFactorN NM1; +typedef NoiseModelFactorN NM2; noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(3); int main(int argc, char *argv[]) {