diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 7ac2de8b1..1dcca5244 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 d9b205359..7a39a4af5 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 5d4ed6657..9aa697f14 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 7bae41403..69975b620 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 f7eddd414..b68d6a97c 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 cb8412cdf..b9f4b0a3e 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 CLANG_DIAGNOSTIC_POP() +# 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 DIAGNOSTIC_POP() #endif namespace gtsam { diff --git a/gtsam/base/utilities.h b/gtsam/base/utilities.h index d9b92b8aa..0a05a704c 100644 --- a/gtsam/base/utilities.h +++ b/gtsam/base/utilities.h @@ -27,3 +27,42 @@ private: }; } + +// boost::index_sequence was introduced in 1.66, so we'll manually define an +// implementation if user has 1.65. boost::index_sequence is used to get array +// indices that align with a parameter pack. +#include +#if BOOST_VERSION >= 106600 +#include +#else +namespace boost { +namespace mp11 { +// Adapted from https://stackoverflow.com/a/32223343/9151520 +template +struct index_sequence { + using type = index_sequence; + using value_type = size_t; + static constexpr std::size_t size() noexcept { return sizeof...(Ints); } +}; +namespace detail { +template +struct _merge_and_renumber; + +template +struct _merge_and_renumber, index_sequence > + : index_sequence {}; +} // namespace detail +template +struct make_index_sequence + : detail::_merge_and_renumber< + typename make_index_sequence::type, + typename make_index_sequence::type> {}; +template <> +struct make_index_sequence<0> : index_sequence<> {}; +template <> +struct make_index_sequence<1> : index_sequence<0> {}; +template +using index_sequence_for = make_index_sequence; +} // namespace mp11 +} // namespace boost +#endif diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 0b6fcdff7..e065bb3f4 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -85,8 +85,8 @@ size_t GaussianMixture::nrComponents() const { /* *******************************************************************************/ GaussianConditional::shared_ptr GaussianMixture::operator()( - const DiscreteValues &discreteVals) const { - auto &ptr = conditionals_(discreteVals); + const DiscreteValues &discreteValues) const { + auto &ptr = conditionals_(discreteValues); if (!ptr) return nullptr; auto conditional = boost::dynamic_pointer_cast(ptr); if (conditional) @@ -209,14 +209,15 @@ void GaussianMixture::prune(const DecisionTreeFactor &decisionTree) { /* *******************************************************************************/ AlgebraicDecisionTree GaussianMixture::error( - const VectorValues &continuousVals) const { - // functor to convert from GaussianConditional to double error value. + const VectorValues &continuousValues) const { + // functor to calculate to double error value from GaussianConditional. auto errorFunc = - [continuousVals](const GaussianConditional::shared_ptr &conditional) { + [continuousValues](const GaussianConditional::shared_ptr &conditional) { if (conditional) { - return conditional->error(continuousVals); + return conditional->error(continuousValues); } else { - // return arbitrarily large error + // Return arbitrarily large error if conditional is null + // Conditional is null if it is pruned out. return 1e50; } }; @@ -225,10 +226,11 @@ AlgebraicDecisionTree GaussianMixture::error( } /* *******************************************************************************/ -double GaussianMixture::error(const VectorValues &continuousVals, +double GaussianMixture::error(const VectorValues &continuousValues, const DiscreteValues &discreteValues) const { + // Directly index to get the conditional, no need to build the whole tree. auto conditional = conditionals_(discreteValues); - return conditional->error(continuousVals); + return conditional->error(continuousValues); } } // namespace gtsam diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index 4bb13d298..88d5a02c0 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -122,7 +122,7 @@ class GTSAM_EXPORT GaussianMixture /// @{ GaussianConditional::shared_ptr operator()( - const DiscreteValues &discreteVals) const; + const DiscreteValues &discreteValues) const; /// Returns the total number of continuous components size_t nrComponents() const; @@ -147,21 +147,21 @@ class GTSAM_EXPORT GaussianMixture /** * @brief Compute error of the GaussianMixture as a tree. * - * @param continuousVals The continuous VectorValues. - * @return AlgebraicDecisionTree A decision tree with corresponding keys - * as the factor but leaf values as the error. + * @param continuousValues The continuous VectorValues. + * @return AlgebraicDecisionTree A decision tree with the same keys + * as the conditionals, and leaf values as the error. */ - AlgebraicDecisionTree error(const VectorValues &continuousVals) const; + AlgebraicDecisionTree error(const VectorValues &continuousValues) const; /** * @brief Compute the error of this Gaussian Mixture given the continuous * values and a discrete assignment. * - * @param continuousVals The continuous values at which to compute the error. + * @param continuousValues Continuous values at which to compute the error. * @param discreteValues The discrete assignment for a specific mode sequence. * @return double */ - double error(const VectorValues &continuousVals, + double error(const VectorValues &continuousValues, const DiscreteValues &discreteValues) const; /** diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index 16802516e..fd437f52c 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -98,21 +98,23 @@ GaussianMixtureFactor::Sum GaussianMixtureFactor::asGaussianFactorGraphTree() /* *******************************************************************************/ AlgebraicDecisionTree GaussianMixtureFactor::error( - const VectorValues &continuousVals) const { + const VectorValues &continuousValues) const { // functor to convert from sharedFactor to double error value. - auto errorFunc = [continuousVals](const GaussianFactor::shared_ptr &factor) { - return factor->error(continuousVals); - }; + auto errorFunc = + [continuousValues](const GaussianFactor::shared_ptr &factor) { + return factor->error(continuousValues); + }; DecisionTree errorTree(factors_, errorFunc); return errorTree; } /* *******************************************************************************/ double GaussianMixtureFactor::error( - const VectorValues &continuousVals, + const VectorValues &continuousValues, const DiscreteValues &discreteValues) const { + // Directly index to get the conditional, no need to build the whole tree. auto factor = factors_(discreteValues); - return factor->error(continuousVals); + return factor->error(continuousValues); } } // namespace gtsam diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index 2808ec49f..0b65b5aa9 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -133,21 +133,21 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { /** * @brief Compute error of the GaussianMixtureFactor as a tree. * - * @param continuousVals The continuous VectorValues. - * @return AlgebraicDecisionTree A decision tree with corresponding keys - * as the factor but leaf values as the error. + * @param continuousValues The continuous VectorValues. + * @return AlgebraicDecisionTree A decision tree with the same keys + * as the factors involved, and leaf values as the error. */ - AlgebraicDecisionTree error(const VectorValues &continuousVals) const; + AlgebraicDecisionTree error(const VectorValues &continuousValues) const; /** * @brief Compute the error of this Gaussian Mixture given the continuous * values and a discrete assignment. * - * @param continuousVals The continuous values at which to compute the error. + * @param continuousValues Continuous values at which to compute the error. * @param discreteValues The discrete assignment for a specific mode sequence. * @return double */ - double error(const VectorValues &continuousVals, + double error(const VectorValues &continuousValues, const DiscreteValues &discreteValues) const; /// Add MixtureFactor to a Sum, syntactic sugar. diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 7338873bc..77e036886 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -250,13 +250,16 @@ AlgebraicDecisionTree HybridBayesNet::error( const VectorValues &continuousValues) const { AlgebraicDecisionTree error_tree; + // Iterate over each factor. for (size_t idx = 0; idx < size(); idx++) { AlgebraicDecisionTree conditional_error; + if (factors_.at(idx)->isHybrid()) { - // If factor is hybrid, select based on assignment. + // If factor is hybrid, select based on assignment and compute error. GaussianMixture::shared_ptr gm = this->atMixture(idx); conditional_error = gm->error(continuousValues); + // Assign for the first index, add error for subsequent ones. if (idx == 0) { error_tree = conditional_error; } else { @@ -267,6 +270,7 @@ AlgebraicDecisionTree HybridBayesNet::error( // If continuous only, get the (double) error // and add it to the error_tree double error = this->atGaussian(idx)->error(continuousValues); + // Add the computed error to every leaf of the error tree. error_tree = error_tree.apply( [error](double leaf_value) { return leaf_value + error; }); @@ -279,6 +283,7 @@ AlgebraicDecisionTree HybridBayesNet::error( return error_tree; } +/* ************************************************************************* */ AlgebraicDecisionTree HybridBayesNet::probPrime( const VectorValues &continuousValues) const { AlgebraicDecisionTree error_tree = this->error(continuousValues); diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index c6ac6dcec..f8ec60911 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -145,8 +145,10 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { AlgebraicDecisionTree error(const VectorValues &continuousValues) const; /** - * @brief Compute unnormalized probability for each discrete assignment, - * and return as a tree. + * @brief Compute unnormalized probability q(μ|M), + * for each discrete assignment, and return as a tree. + * q(μ|M) is the unnormalized probability at the MLE point μ, + * conditioned on the discrete variables. * * @param continuousValues Continuous values at which to compute the * probability. diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index c8707a586..39c3c2965 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -448,6 +448,7 @@ AlgebraicDecisionTree HybridGaussianFactorGraph::error( const VectorValues &continuousValues) const { AlgebraicDecisionTree error_tree(0.0); + // Iterate over each factor. for (size_t idx = 0; idx < size(); idx++) { AlgebraicDecisionTree factor_error; @@ -455,8 +456,10 @@ AlgebraicDecisionTree HybridGaussianFactorGraph::error( // If factor is hybrid, select based on assignment. GaussianMixtureFactor::shared_ptr gaussianMixture = boost::static_pointer_cast(factors_.at(idx)); + // Compute factor error. factor_error = gaussianMixture->error(continuousValues); + // If first factor, assign error, else add it. if (idx == 0) { error_tree = factor_error; } else { @@ -470,7 +473,9 @@ AlgebraicDecisionTree HybridGaussianFactorGraph::error( boost::static_pointer_cast(factors_.at(idx)); GaussianFactor::shared_ptr gaussian = hybridGaussianFactor->inner(); + // Compute the error of the gaussian factor. double error = gaussian->error(continuousValues); + // Add the gaussian factor error to every leaf of the error tree. error_tree = error_tree.apply( [error](double leaf_value) { return leaf_value + error; }); diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index 210ce50e9..b3bf8c0f5 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -100,11 +100,12 @@ class GTSAM_EXPORT HybridGaussianFactorGraph using shared_ptr = boost::shared_ptr; ///< shared_ptr to This using Values = gtsam::Values; ///< backwards compatibility - using Indices = KeyVector; ///> map from keys to values + using Indices = KeyVector; ///< map from keys to values /// @name Constructors /// @{ + /// @brief Default constructor. HybridGaussianFactorGraph() = default; /** @@ -175,6 +176,8 @@ class GTSAM_EXPORT HybridGaussianFactorGraph * @brief Compute error for each discrete assignment, * and return as a tree. * + * Error \f$ e = \Vert x - \mu \Vert_{\Sigma} \f$. + * * @param continuousValues Continuous values at which to compute the error. * @return AlgebraicDecisionTree */ @@ -194,8 +197,8 @@ class GTSAM_EXPORT HybridGaussianFactorGraph const DiscreteValues& discreteValues) const; /** - * @brief Compute unnormalized probability for each discrete assignment, - * and return as a tree. + * @brief Compute unnormalized probability \f$ P(X | M, Z) \f$ + * for each discrete assignment, and return as a tree. * * @param continuousValues Continuous values at which to compute the * probability. diff --git a/gtsam/hybrid/MixtureFactor.h b/gtsam/hybrid/MixtureFactor.h index 511705cf3..f29a84022 100644 --- a/gtsam/hybrid/MixtureFactor.h +++ b/gtsam/hybrid/MixtureFactor.h @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -86,11 +87,11 @@ class MixtureFactor : public HybridFactor { * elements based on the number of discrete keys and the cardinality of the * keys, so that the decision tree is constructed appropriately. * - * @tparam FACTOR The type of the factor shared pointers being passed in. Will - * be typecast to NonlinearFactor shared pointers. + * @tparam FACTOR The type of the factor shared pointers being passed in. + * Will be typecast to NonlinearFactor shared pointers. * @param keys Vector of keys for continuous factors. * @param discreteKeys Vector of discrete keys. - * @param factors Vector of shared pointers to factors. + * @param factors Vector of nonlinear factors. * @param normalized Flag indicating if the factor error is already * normalized. */ @@ -128,14 +129,15 @@ class MixtureFactor : public HybridFactor { /** * @brief Compute error of the MixtureFactor as a tree. * - * @param continuousVals The continuous values for which to compute the error. - * @return AlgebraicDecisionTree A decision tree with corresponding keys - * as the factor but leaf values as the error. + * @param continuousValues The continuous values for which to compute the + * error. + * @return AlgebraicDecisionTree A decision tree with the same keys + * as the factor, and leaf values as the error. */ - AlgebraicDecisionTree error(const Values& continuousVals) const { + AlgebraicDecisionTree error(const Values& continuousValues) const { // functor to convert from sharedFactor to double error value. - auto errorFunc = [continuousVals](const sharedFactor& factor) { - return factor->error(continuousVals); + auto errorFunc = [continuousValues](const sharedFactor& factor) { + return factor->error(continuousValues); }; DecisionTree errorTree(factors_, errorFunc); return errorTree; @@ -144,19 +146,19 @@ class MixtureFactor : public HybridFactor { /** * @brief Compute error of factor given both continuous and discrete values. * - * @param continuousVals The continuous Values. - * @param discreteVals The discrete Values. + * @param continuousValues The continuous Values. + * @param discreteValues The discrete Values. * @return double The error of this factor. */ - double error(const Values& continuousVals, - const DiscreteValues& discreteVals) const { - // Retrieve the factor corresponding to the assignment in discreteVals. - auto factor = factors_(discreteVals); + double error(const Values& continuousValues, + const DiscreteValues& discreteValues) const { + // Retrieve the factor corresponding to the assignment in discreteValues. + auto factor = factors_(discreteValues); // Compute the error for the selected factor - const double factorError = factor->error(continuousVals); + const double factorError = factor->error(continuousValues); if (normalized_) return factorError; - return factorError + - this->nonlinearFactorLogNormalizingConstant(factor, continuousVals); + return factorError + this->nonlinearFactorLogNormalizingConstant( + factor, continuousValues); } size_t dim() const { @@ -212,17 +214,18 @@ class MixtureFactor : public HybridFactor { /// Linearize specific nonlinear factors based on the assignment in /// discreteValues. GaussianFactor::shared_ptr linearize( - const Values& continuousVals, const DiscreteValues& discreteVals) const { - auto factor = factors_(discreteVals); - return factor->linearize(continuousVals); + const Values& continuousValues, + const DiscreteValues& discreteValues) const { + auto factor = factors_(discreteValues); + return factor->linearize(continuousValues); } /// Linearize all the continuous factors to get a GaussianMixtureFactor. boost::shared_ptr linearize( - const Values& continuousVals) const { + const Values& continuousValues) const { // functional to linearize each factor in the decision tree - auto linearizeDT = [continuousVals](const sharedFactor& factor) { - return factor->linearize(continuousVals); + auto linearizeDT = [continuousValues](const sharedFactor& factor) { + return factor->linearize(continuousValues); }; DecisionTree linearized_factors( diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 86029a48a..899c129e0 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -196,22 +196,24 @@ class HybridNonlinearFactorGraph { #include class MixtureFactor : gtsam::HybridFactor { - MixtureFactor(const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys, - const gtsam::DecisionTree& factors, bool normalized = false); + MixtureFactor( + const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys, + const gtsam::DecisionTree& factors, + bool normalized = false); template MixtureFactor(const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys, const std::vector& factors, bool normalized = false); - double error(const gtsam::Values& continuousVals, - const gtsam::DiscreteValues& discreteVals) const; + double error(const gtsam::Values& continuousValues, + const gtsam::DiscreteValues& discreteValues) const; double nonlinearFactorLogNormalizingConstant(const gtsam::NonlinearFactor* factor, const gtsam::Values& values) const; GaussianMixtureFactor* linearize( - const gtsam::Values& continuousVals) const; + const gtsam::Values& continuousValues) const; void print(string s = "MixtureFactor\n", const gtsam::KeyFormatter& keyFormatter = diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp index 556a5f16a..310081f02 100644 --- a/gtsam/hybrid/tests/testGaussianMixture.cpp +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -104,7 +104,7 @@ TEST(GaussianMixture, Error) { X(2), S2, model); // Create decision tree - DiscreteKey m1(1, 2); + DiscreteKey m1(M(1), 2); GaussianMixture::Conditionals conditionals( {m1}, vector{conditional0, conditional1}); @@ -115,12 +115,19 @@ TEST(GaussianMixture, Error) { values.insert(X(2), Vector2::Zero()); auto error_tree = mixture.error(values); + // regression std::vector discrete_keys = {m1}; std::vector leaves = {0.5, 4.3252595}; AlgebraicDecisionTree expected_error(discrete_keys, leaves); - // regression EXPECT(assert_equal(expected_error, error_tree, 1e-6)); + + // Regression for non-tree version. + DiscreteValues assignment; + assignment[M(1)] = 0; + EXPECT_DOUBLES_EQUAL(0.5, mixture.error(values, assignment), 1e-8); + assignment[M(1)] = 1; + EXPECT_DOUBLES_EQUAL(4.3252595155709335, mixture.error(values, assignment), 1e-8); } /* ************************************************************************* */ diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 5c25a0931..ba0622ff9 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -170,24 +170,25 @@ TEST(GaussianMixtureFactor, Error) { GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors); - VectorValues continuousVals; - continuousVals.insert(X(1), Vector2(0, 0)); - continuousVals.insert(X(2), Vector2(1, 1)); + VectorValues continuousValues; + continuousValues.insert(X(1), Vector2(0, 0)); + continuousValues.insert(X(2), Vector2(1, 1)); // error should return a tree of errors, with nodes for each discrete value. - AlgebraicDecisionTree error_tree = mixtureFactor.error(continuousVals); + AlgebraicDecisionTree error_tree = mixtureFactor.error(continuousValues); std::vector discrete_keys = {m1}; + // Error values for regression test std::vector errors = {1, 4}; AlgebraicDecisionTree expected_error(discrete_keys, errors); EXPECT(assert_equal(expected_error, error_tree)); // Test for single leaf given discrete assignment P(X|M,Z). - DiscreteValues discreteVals; - discreteVals[m1.first] = 1; - EXPECT_DOUBLES_EQUAL(4.0, mixtureFactor.error(continuousVals, discreteVals), - 1e-9); + DiscreteValues discreteValues; + discreteValues[m1.first] = 1; + EXPECT_DOUBLES_EQUAL( + 4.0, mixtureFactor.error(continuousValues, discreteValues), 1e-9); } /* ************************************************************************* */ diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index d2951ddaa..a8ea6605b 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -197,8 +197,7 @@ TEST(HybridBayesNet, Error) { // Verify error computation and check for specific error value DiscreteValues discrete_values; - discrete_values[M(0)] = 1; - discrete_values[M(1)] = 1; + boost::assign::insert(discrete_values)(M(0), 1)(M(1), 1); double total_error = 0; for (size_t idx = 0; idx < hybridBayesNet->size(); idx++) { diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp index c00d70e5a..fe3212eda 100644 --- a/gtsam/hybrid/tests/testMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testMixtureFactor.cpp @@ -41,7 +41,8 @@ TEST(MixtureFactor, Constructor) { CHECK(it == factor.end()); } - +/* ************************************************************************* */ +// Test .print() output. TEST(MixtureFactor, Printing) { DiscreteKey m1(1, 2); double between0 = 0.0; @@ -87,11 +88,11 @@ TEST(MixtureFactor, Error) { MixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors); - Values continuousVals; - continuousVals.insert(X(1), 0); - continuousVals.insert(X(2), 1); + Values continuousValues; + continuousValues.insert(X(1), 0); + continuousValues.insert(X(2), 1); - AlgebraicDecisionTree error_tree = mixtureFactor.error(continuousVals); + AlgebraicDecisionTree error_tree = mixtureFactor.error(continuousValues); std::vector discrete_keys = {m1}; std::vector errors = {0.5, 0}; diff --git a/gtsam/inference/graph-inl.h b/gtsam/inference/graph-inl.h index 3ea66405b..a23eda60c 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/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 6dcf662a9..41a734b34 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -217,14 +217,7 @@ namespace gtsam { double GaussianBayesNet::logDeterminant() const { double logDet = 0.0; for (const sharedConditional& cg : *this) { - if (cg->get_model()) { - Vector diag = cg->R().diagonal(); - cg->get_model()->whitenInPlace(diag); - logDet += diag.unaryExpr([](double x) { return log(x); }).sum(); - } else { - logDet += - cg->R().diagonal().unaryExpr([](double x) { return log(x); }).sum(); - } + logDet += cg->logDeterminant(); } return logDet; } diff --git a/gtsam/linear/GaussianBayesTree-inl.h b/gtsam/linear/GaussianBayesTree-inl.h index d781533e6..e73420644 100644 --- a/gtsam/linear/GaussianBayesTree-inl.h +++ b/gtsam/linear/GaussianBayesTree-inl.h @@ -43,7 +43,7 @@ double logDeterminant(const typename BAYESTREE::sharedClique& clique) { double result = 0.0; // this clique - result += clique->conditional()->R().diagonal().unaryExpr(std::ptr_fun(log)).sum(); + result += clique->conditional()->logDeterminant(); // sum of children for(const typename BAYESTREE::sharedClique& child: clique->children_) diff --git a/gtsam/linear/GaussianBayesTree.cpp b/gtsam/linear/GaussianBayesTree.cpp index a83475e26..b35252e72 100644 --- a/gtsam/linear/GaussianBayesTree.cpp +++ b/gtsam/linear/GaussianBayesTree.cpp @@ -50,15 +50,7 @@ namespace gtsam { const GaussianBayesTreeClique::shared_ptr& clique, LogDeterminantData& parentSum) { auto cg = clique->conditional(); - double logDet; - if (cg->get_model()) { - Vector diag = cg->R().diagonal(); - cg->get_model()->whitenInPlace(diag); - logDet = diag.unaryExpr([](double x) { return log(x); }).sum(); - } else { - logDet = - cg->R().diagonal().unaryExpr([](double x) { return log(x); }).sum(); - } + double logDet = cg->logDeterminant(); // Add the current clique's log-determinant to the overall sum (*parentSum.logDet) += logDet; return parentSum; diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 951577641..60ddb1b7d 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -155,6 +155,20 @@ namespace gtsam { } } +/* ************************************************************************* */ +double GaussianConditional::logDeterminant() const { + double logDet; + if (this->get_model()) { + Vector diag = this->R().diagonal(); + this->get_model()->whitenInPlace(diag); + logDet = diag.unaryExpr([](double x) { return log(x); }).sum(); + } else { + logDet = + this->R().diagonal().unaryExpr([](double x) { return log(x); }).sum(); + } + return logDet; +} + /* ************************************************************************* */ VectorValues GaussianConditional::solve(const VectorValues& x) const { // Concatenate all vector values that correspond to parent variables diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 4822e3eaf..8af7f6602 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -133,6 +133,28 @@ namespace gtsam { /** Get a view of the r.h.s. vector d */ const constBVector d() const { return BaseFactor::getb(); } + /** + * @brief Compute the log determinant of the Gaussian conditional. + * The determinant is computed using the R matrix, which is upper + * triangular. + * For numerical stability, the determinant is computed in log + * form, so it is a summation rather than a multiplication. + * + * @return double + */ + double logDeterminant() const; + + /** + * @brief Compute the determinant of the conditional from the + * upper-triangular R matrix. + * + * The determinant is computed in log form (hence summation) for numerical + * stability and then exponentiated. + * + * @return double + */ + double determinant() const { return exp(this->logDeterminant()); } + /** * Solves a conditional Gaussian and writes the solution into the entries of * \c x for each frontal variable of the conditional. The parents are diff --git a/gtsam/mainpage.dox b/gtsam/mainpage.dox index 3c5342ca5..65290e0c9 100644 --- a/gtsam/mainpage.dox +++ b/gtsam/mainpage.dox @@ -22,7 +22,7 @@ To use GTSAM to solve your own problems, you will often have to create new facto -# The number of variables your factor involves is unknown at compile time - derive from NoiseModelFactor and implement NoiseModelFactor::unwhitenedError() - This is a factor expressing the sum-of-squares error between a measurement \f$ z \f$ and a measurement prediction function \f$ h(x) \f$, on which the errors are expected to follow some distribution specified by a noise model (see noiseModel). --# The number of variables your factor involves is known at compile time and is between 1 and 6 - derive from NoiseModelFactor1, NoiseModelFactor2, NoiseModelFactor3, NoiseModelFactor4, NoiseModelFactor5, or NoiseModelFactor6, and implement \c evaluateError() +-# The number of variables your factor involves is known at compile time, derive from NoiseModelFactorN (where T1, T2, ... are the types of the variables, e.g. double, Vector, Pose3) and implement \c evaluateError(). - This factor expresses the same sum-of-squares error with a noise model, but makes the implementation task slightly easier than with %NoiseModelFactor. -# Derive from NonlinearFactor - This is more advanced and allows creating factors without an explicit noise model, or that linearize to HessianFactor instead of JacobianFactor. diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index f4db42d0f..afaaee1d4 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 c7d82975a..87fdd2e91 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -128,10 +128,10 @@ private: } }; -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 @@ private: 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 e8da541b1..93495f227 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -76,9 +76,9 @@ public: * 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 @@ private: 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 @@ private: 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 2f0ff7436..2e87986ae 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 b570049a5..2d793c475 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 8d3a7dd31..38b3dc763 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 5591bb357..3448e7794 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -255,14 +255,14 @@ public: * * @ingroup navigation */ -class GTSAM_EXPORT CombinedImuFactor: public NoiseModelFactor6 { public: private: typedef CombinedImuFactor This; - typedef NoiseModelFactor6 Base; PreintegratedCombinedMeasurements _PIM_; @@ -334,7 +334,9 @@ public: friend class boost::serialization::access; template 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 6ab4c2f02..4db2b82c0 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 40d0ef22a..e515d9012 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 @@ private: 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 @@ private: * 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 @@ private: 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 9b6affaaf..1b07991e9 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 bffe3e645..feae1eb14 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -168,12 +168,12 @@ public: * * @ingroup navigation */ -class GTSAM_EXPORT ImuFactor: public NoiseModelFactor5 { private: typedef ImuFactor This; - typedef NoiseModelFactor5 Base; PreintegratedImuMeasurements _PIM_; @@ -248,6 +248,7 @@ public: friend class boost::serialization::access; template 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 @@ public: * ImuFactor2 is a ternary factor that uses NavStates rather than Pose/Velocity. * @ingroup navigation */ -class GTSAM_EXPORT ImuFactor2 : public NoiseModelFactor3 { +class GTSAM_EXPORT ImuFactor2 : public NoiseModelFactorN { private: typedef ImuFactor2 This; - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; PreintegratedImuMeasurements _PIM_; @@ -316,6 +317,7 @@ private: 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(_PIM_); diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index 895ac6512..9a718a5e1 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 @@ public: 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 @@ public: * 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 @@ public: 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 @@ public: * 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 @@ public: /** 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 @@ public: * 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 @@ public: /** 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 da2a54ce9..c817e22b4 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 dc51de7bb..d76a6ea7e 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 394b22b6b..1fb44ebf7 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 43d30254e..d1aa58b27 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 @@ private: using This = NonlinearEquality; // typedef to base class - using Base = NoiseModelFactor1; + using Base = NoiseModelFactorN; public: @@ -184,6 +184,7 @@ private: 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 @@ private: 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 7fafd95df..c58179db3 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 @@ -25,6 +26,7 @@ #include #include #include +#include // boost::index_sequence #include #include @@ -272,68 +274,194 @@ public: /* ************************************************************************* */ - /** - * A convenient base class for creating your own NoiseModelFactor with 1 - * variable. To derive from this class, implement evaluateError(). + * A convenient base class for creating your own NoiseModelFactor + * with n variables. To derive from this class, implement evaluateError(). * - * Templated on a values structure type. The values structures are typically - * more general than just vectors, e.g., Rot3 or Pose3, - * which are objects in non-linear manifolds (Lie groups). + * For example, a 2-way factor that computes the difference in x-translation + * between a Pose3 and Point3 could be implemented like so: + * + * ~~~~~~~~~~~~~~~~~~~~{.cpp} + * class MyFactor : public NoiseModelFactorN { + * public: + * using Base = NoiseModelFactorN; + * + * MyFactor(Key pose_key, Key point_key, const SharedNoiseModel& noiseModel) + * : Base(noiseModel, pose_key, point_key) {} + * + * Vector evaluateError( + * const Pose3& T, const Point3& p, + * boost::optional H_T = boost::none, + * boost::optional H_p = boost::none) const override { + * Matrix36 t_H_T; // partial derivative of translation w.r.t. pose T + * + * // Only compute t_H_T if needed: + * Point3 t = T.translation(H_T ? &t_H_T : 0); + * double a = t(0); // a_H_t = [1, 0, 0] + * double b = p(0); // b_H_p = [1, 0, 0] + * double error = a - b; // H_a = 1, H_b = -1 + * + * // H_T = H_a * a_H_t * t_H_T = the first row of t_H_T + * if (H_T) *H_T = (Matrix(1, 6) << t_H_T.row(0)).finished(); + * // H_p = H_b * b_H_p = -1 * [1, 0, 0] + * if (H_p) *H_p = (Matrix(1, 3) << -1., 0., 0.).finished(); + * + * return Vector1(error); + * } + * }; + * + * // Unit Test + * TEST(NonlinearFactor, MyFactor) { + * MyFactor f(X(1), X(2), noiseModel::Unit::Create(1)); + * EXPECT_DOUBLES_EQUAL(-8., f.evaluateError(Pose3(), Point3(8., 7., 6.))(0), + * 1e-9); + * Values values; + * values.insert(X(1), Pose3(Rot3::RzRyRx(0.1, 0.2, 0.3), Point3(1, 2, 3))); + * values.insert(X(2), Point3(1, 2, 3)); + * EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-5); + * } + * ~~~~~~~~~~~~~~~~~~~~ + * + * These factors are templated on a values structure type. The values structures + * are typically more general than just vectors, e.g., Rot3 or Pose3, which are + * objects in non-linear manifolds (Lie groups). */ -template -class NoiseModelFactor1: public NoiseModelFactor { +template +class NoiseModelFactorN : public NoiseModelFactor { + public: + /// N is the number of variables (N-way factor) + enum { N = sizeof...(ValueTypes) }; -public: + protected: + using Base = NoiseModelFactor; + using This = NoiseModelFactorN; - // typedefs for value types pulled from keys - typedef VALUE X; + /// @name SFINAE aliases + /// @{ -protected: + template + using IsConvertible = + typename std::enable_if::value, void>::type; - typedef NoiseModelFactor Base; - typedef NoiseModelFactor1 This; + template + using IndexIsValid = typename std::enable_if<(I >= 1) && (I <= N), + void>::type; // 1-indexed! + + template + using ContainerElementType = + typename std::decay().begin())>::type; + template + using IsContainerOfKeys = IsConvertible, Key>; + + /// @} + + /* Like std::void_t, except produces `boost::optional` instead of + * `void`. Used to expand fixed-type parameter-packs with same length as + * ValueTypes. */ + template + using OptionalMatrix = boost::optional; + + /* Like std::void_t, except produces `Key` instead of `void`. Used to expand + * fixed-type parameter-packs with same length as ValueTypes. */ + template + using KeyType = Key; + + public: + /** + * The type of the I'th template param can be obtained as ValueType. + * I is 1-indexed for backwards compatibility/consistency! So for example, + * ``` + * using Factor = NoiseModelFactorN; + * Factor::ValueType<1> // Pose3 + * Factor::ValueType<2> // Point3 + * // Factor::ValueType<0> // ERROR! Will not compile. + * // Factor::ValueType<3> // ERROR! Will not compile. + * ``` + */ + template > + using ValueType = + typename std::tuple_element>::type; + + public: -public: /// @name Constructors /// @{ - /** Default constructor for I/O only */ - NoiseModelFactor1() {} - - ~NoiseModelFactor1() override {} - - inline Key key() const { return keys_[0]; } + /// Default Constructor for I/O + NoiseModelFactorN() {} /** - * Constructor - * @param noiseModel shared pointer to noise model - * @param key1 by which to look up X value in Values + * Constructor. + * Example usage: NoiseModelFactorN(noise, key1, key2, ..., keyN) + * @param noiseModel Shared pointer to noise model. + * @param keys Keys for the variables in this factor, passed in as separate + * arguments. */ - NoiseModelFactor1(const SharedNoiseModel &noiseModel, Key key1) - : Base(noiseModel, cref_list_of<1>(key1)) {} + NoiseModelFactorN(const SharedNoiseModel& noiseModel, + KeyType... keys) + : Base(noiseModel, std::array{keys...}) {} + + /** + * Constructor. + * Example usage: `NoiseModelFactorN(noise, {key1, key2, ..., keyN})` + * Example usage: `NoiseModelFactorN(noise, keys)` where keys is a vector + * @param noiseModel Shared pointer to noise model. + * @param keys A container of keys for the variables in this factor. + */ + template , + typename = IsContainerOfKeys> + NoiseModelFactorN(const SharedNoiseModel& noiseModel, CONTAINER keys) + : Base(noiseModel, keys) { + if (keys.size() != N) { + throw std::invalid_argument( + "NoiseModelFactorN: wrong number of keys given"); + } + } /// @} + + ~NoiseModelFactorN() override {} + + /** Returns a key. Usage: `key()` returns the I'th key. + * I is 1-indexed for backwards compatibility/consistency! So for example, + * ``` + * NoiseModelFactorN factor(noise, key1, key2); + * key<1>() // = key1 + * key<2>() // = key2 + * // key<0>() // ERROR! Will not compile + * // key<3>() // ERROR! Will not compile + * ``` + */ + template + inline Key key() const { + static_assert(I <= N, "Index out of bounds"); + return keys_[I - 1]; + } + /// @name NoiseModelFactor methods /// @{ - /** - * Calls the 1-key specific version of evaluateError below, which is pure - * virtual so must be implemented in the derived class. + /** This implements the `unwhitenedError` virtual function by calling the + * n-key specific version of evaluateError, which is pure virtual so must be + * implemented in the derived class. + * + * Example usage: + * ``` + * gtsam::Values values; + * values.insert(...) // populate values + * std::vector Hs(2); // this will be an optional output argument + * const Vector error = factor.unwhitenedError(values, Hs); + * ``` + * @param[in] x A Values object containing the values of all the variables + * used in this factor + * @param[out] H A vector of (dynamic) matrices whose size should be equal to + * n. The Jacobians w.r.t. each variable will be output in this parameter. */ Vector unwhitenedError( - const Values &x, - boost::optional &> H = boost::none) const override { - if (this->active(x)) { - const X &x1 = x.at(keys_[0]); - if (H) { - return evaluateError(x1, (*H)[0]); - } else { - return evaluateError(x1); - } - } else { - return Vector::Zero(this->dim()); - } + const Values& x, + boost::optional&> H = boost::none) const override { + return unwhitenedError(boost::mp11::index_sequence_for{}, x, + H); } /// @} @@ -341,434 +469,393 @@ public: /// @{ /** - * Override this method to finish implementing a unary factor. - * If the optional Matrix reference argument is specified, it should compute - * both the function evaluation and its derivative in X. + * Override `evaluateError` to finish implementing an n-way factor. + * + * Both the `x` and `H` arguments are written here as parameter packs, but + * when overriding this method, you probably want to explicitly write them + * out. For example, for a 2-way factor with variable types Pose3 and Point3, + * you should implement: + * ``` + * Vector evaluateError( + * const Pose3& x1, const Point3& x2, + * boost::optional H1 = boost::none, + * boost::optional H2 = boost::none) const override { ... } + * ``` + * + * If any of the optional Matrix reference arguments are specified, it should + * compute both the function evaluation and its derivative(s) in the requested + * variables. + * + * @param x The values of the variables to evaluate the error for. Passed in + * as separate arguments. + * @param[out] H The Jacobian with respect to each variable (optional). */ - virtual Vector - evaluateError(const X &x, - boost::optional H = boost::none) const = 0; + virtual Vector evaluateError(const ValueTypes&... x, + OptionalMatrix... H) const = 0; /// @} -private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("NoiseModelFactor", - boost::serialization::base_object(*this)); + /// @name Convenience method overloads + /// @{ + + /** No-Jacobians requested function overload. + * This specializes the version below to avoid recursive calls since this is + * commonly used. + * + * e.g. `const Vector error = factor.evaluateError(pose, point);` + */ + inline Vector evaluateError(const ValueTypes&... x) const { + return evaluateError(x..., OptionalMatrix()...); } -};// \class NoiseModelFactor1 - -/* ************************************************************************* */ -/** A convenient base class for creating your own NoiseModelFactor with 2 - * variables. To derive from this class, implement evaluateError(). */ -template -class NoiseModelFactor2: public NoiseModelFactor { - -public: - - // typedefs for value types pulled from keys - typedef VALUE1 X1; - typedef VALUE2 X2; - -protected: - - typedef NoiseModelFactor Base; - typedef NoiseModelFactor2 This; - -public: - - /** - * Default Constructor for I/O + /** Some (but not all) optional Jacobians are omitted (function overload) + * + * e.g. `const Vector error = factor.evaluateError(pose, point, Hpose);` */ - NoiseModelFactor2() {} + template > + inline Vector evaluateError(const ValueTypes&... x, + OptionalJacArgs&&... H) const { + return evaluateError(x..., std::forward(H)..., + boost::none); + } - /** - * Constructor - * @param noiseModel shared pointer to noise model - * @param j1 key of the first variable - * @param j2 key of the second variable + /// @} + + private: + /** Pack expansion with index_sequence template pattern, used to index into + * `keys_` and `H`. + * + * Example: For `NoiseModelFactorN`, the call would look like: + * `const Vector error = unwhitenedError(0, 1, values, H);` */ - NoiseModelFactor2(const SharedNoiseModel& noiseModel, Key j1, Key j2) : - Base(noiseModel, cref_list_of<2>(j1)(j2)) {} - - ~NoiseModelFactor2() override {} - - /** methods to retrieve both keys */ - inline Key key1() const { return keys_[0]; } - inline Key key2() const { return keys_[1]; } - - /** Calls the 2-key specific version of evaluateError, which is pure virtual - * so must be implemented in the derived class. */ - Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const override { - if(this->active(x)) { - const X1& x1 = x.at(keys_[0]); - const X2& x2 = x.at(keys_[1]); - if(H) { - return evaluateError(x1, x2, (*H)[0], (*H)[1]); + template + inline Vector unwhitenedError( + boost::mp11::index_sequence, // + const Values& x, + boost::optional&> H = boost::none) const { + if (this->active(x)) { + if (H) { + return evaluateError(x.at(keys_[Indices])..., + (*H)[Indices]...); } else { - return evaluateError(x1, x2); + return evaluateError(x.at(keys_[Indices])...); } } else { return Vector::Zero(this->dim()); } } - /** - * Override this method to finish implementing a binary factor. - * If any of the optional Matrix reference arguments are specified, it should compute - * both the function evaluation and its derivative(s) in X1 (and/or X2). - */ - virtual Vector - evaluateError(const X1&, const X2&, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) const = 0; - -private: - /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("NoiseModelFactor", - boost::serialization::base_object(*this)); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor", boost::serialization::base_object(*this)); } -}; // \class NoiseModelFactor2 + + 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 /* ************************************************************************* */ -/** A convenient base class for creating your own NoiseModelFactor with 3 - * variables. To derive from this class, implement evaluateError(). */ -template -class NoiseModelFactor3: public NoiseModelFactor { - -public: - - // typedefs for value types pulled from keys - typedef VALUE1 X1; - typedef VALUE2 X2; - typedef VALUE3 X3; - -protected: - - typedef NoiseModelFactor Base; - typedef NoiseModelFactor3 This; - -public: - - /** - * Default Constructor for I/O +/** @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(). + */ +template +class GTSAM_DEPRECATED NoiseModelFactor1 : public NoiseModelFactorN { + public: + /** Aliases for value types pulled from keys, for backwards compatibility. + * Note: in your code you can probably just do: + * `using X = ValueType<1>;` + * but this class is uglier due to dependent types. + * See e.g. testNonlinearFactor.cpp:TestFactorN. */ - NoiseModelFactor3() {} + using X = typename NoiseModelFactor1::template ValueType<1>; - /** - * Constructor - * @param noiseModel shared pointer to noise model - * @param j1 key of the first variable - * @param j2 key of the second variable - * @param j3 key of the third variable + protected: + using Base = NoiseModelFactor; // grandparent, for backwards compatibility + using This = NoiseModelFactor1; + + public: + // inherit NoiseModelFactorN's constructors + using NoiseModelFactorN::NoiseModelFactorN; + ~NoiseModelFactor1() override {} + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor", boost::serialization::base_object(*this)); + } +}; // \class NoiseModelFactor1 + +/* ************************************************************************* */ +/** @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(). + */ +template +class GTSAM_DEPRECATED NoiseModelFactor2 + : public NoiseModelFactorN { + public: + /** Aliases for value types pulled from keys. + * Note: in your code you can probably just do: + * `using X1 = ValueType<1>;` + * but this class is uglier due to dependent types. + * See e.g. testNonlinearFactor.cpp:TestFactorN. */ - NoiseModelFactor3(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3) : - Base(noiseModel, cref_list_of<3>(j1)(j2)(j3)) {} + using X1 = typename NoiseModelFactor2::template ValueType<1>; + using X2 = typename NoiseModelFactor2::template ValueType<2>; + protected: + using Base = NoiseModelFactor; + using This = NoiseModelFactor2; + + public: + // inherit NoiseModelFactorN's constructors + using NoiseModelFactorN::NoiseModelFactorN; + ~NoiseModelFactor2() override {} + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor", boost::serialization::base_object(*this)); + } +}; // \class NoiseModelFactor2 + +/* ************************************************************************* */ +/** @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(). + */ +template +class GTSAM_DEPRECATED NoiseModelFactor3 + : public NoiseModelFactorN { + public: + /** Aliases for value types pulled from keys. + * Note: in your code you can probably just do: + * `using X1 = ValueType<1>;` + * but this class is uglier due to dependent types. + * See e.g. testNonlinearFactor.cpp:TestFactorN. + */ + using X1 = typename NoiseModelFactor3::template ValueType<1>; + using X2 = typename NoiseModelFactor3::template ValueType<2>; + using X3 = typename NoiseModelFactor3::template ValueType<3>; + + protected: + using Base = NoiseModelFactor; + using This = NoiseModelFactor3; + + public: + // inherit NoiseModelFactorN's constructors + using NoiseModelFactorN::NoiseModelFactorN; ~NoiseModelFactor3() override {} - /** methods to retrieve keys */ - inline Key key1() const { return keys_[0]; } - inline Key key2() const { return keys_[1]; } - inline Key key3() const { return keys_[2]; } - - /** Calls the 3-key specific version of evaluateError, which is pure virtual - * so must be implemented in the derived class. */ - Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const override { - if(this->active(x)) { - if(H) - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), (*H)[0], (*H)[1], (*H)[2]); - else - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2])); - } else { - return Vector::Zero(this->dim()); - } - } - - /** - * Override this method to finish implementing a trinary factor. - * If any of the optional Matrix reference arguments are specified, it should compute - * both the function evaluation and its derivative(s) in X1 (and/or X2, X3). - */ - virtual Vector - evaluateError(const X1&, const X2&, const X3&, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const = 0; - -private: - + private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("NoiseModelFactor", - boost::serialization::base_object(*this)); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor", boost::serialization::base_object(*this)); } -}; // \class NoiseModelFactor3 +}; // \class NoiseModelFactor3 /* ************************************************************************* */ -/** A convenient base class for creating your own NoiseModelFactor with 4 - * variables. To derive from this class, implement evaluateError(). */ -template -class NoiseModelFactor4: public NoiseModelFactor { - -public: - - // typedefs for value types pulled from keys - typedef VALUE1 X1; - typedef VALUE2 X2; - typedef VALUE3 X3; - typedef VALUE4 X4; - -protected: - - typedef NoiseModelFactor Base; - typedef NoiseModelFactor4 This; - -public: - - /** - * Default Constructor for I/O +/** @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(). + */ +template +class GTSAM_DEPRECATED NoiseModelFactor4 + : public NoiseModelFactorN { + public: + /** Aliases for value types pulled from keys. + * Note: in your code you can probably just do: + * `using X1 = ValueType<1>;` + * but this class is uglier due to dependent types. + * See e.g. testNonlinearFactor.cpp:TestFactorN. */ - NoiseModelFactor4() {} + using X1 = typename NoiseModelFactor4::template ValueType<1>; + using X2 = typename NoiseModelFactor4::template ValueType<2>; + using X3 = typename NoiseModelFactor4::template ValueType<3>; + using X4 = typename NoiseModelFactor4::template ValueType<4>; - /** - * Constructor - * @param noiseModel shared pointer to noise model - * @param j1 key of the first variable - * @param j2 key of the second variable - * @param j3 key of the third variable - * @param j4 key of the fourth variable - */ - NoiseModelFactor4(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4) : - Base(noiseModel, cref_list_of<4>(j1)(j2)(j3)(j4)) {} + protected: + using Base = NoiseModelFactor; + using This = NoiseModelFactor4; + public: + // inherit NoiseModelFactorN's constructors + using NoiseModelFactorN::NoiseModelFactorN; ~NoiseModelFactor4() override {} - /** methods to retrieve keys */ - inline Key key1() const { return keys_[0]; } - inline Key key2() const { return keys_[1]; } - inline Key key3() const { return keys_[2]; } - inline Key key4() const { return keys_[3]; } - - /** Calls the 4-key specific version of evaluateError, which is pure virtual - * so must be implemented in the derived class. */ - Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const override { - if(this->active(x)) { - if(H) - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), (*H)[0], (*H)[1], (*H)[2], (*H)[3]); - else - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3])); - } else { - return Vector::Zero(this->dim()); - } - } - - /** - * Override this method to finish implementing a 4-way factor. - * If any of the optional Matrix reference arguments are specified, it should compute - * both the function evaluation and its derivative(s) in X1 (and/or X2, X3). - */ - virtual Vector - evaluateError(const X1&, const X2&, const X3&, const X4&, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none) const = 0; - -private: - + private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("NoiseModelFactor", - boost::serialization::base_object(*this)); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor", boost::serialization::base_object(*this)); } -}; // \class NoiseModelFactor4 +}; // \class NoiseModelFactor4 /* ************************************************************************* */ -/** A convenient base class for creating your own NoiseModelFactor with 5 - * variables. To derive from this class, implement evaluateError(). */ -template -class NoiseModelFactor5: public NoiseModelFactor { - -public: - - // typedefs for value types pulled from keys - typedef VALUE1 X1; - typedef VALUE2 X2; - typedef VALUE3 X3; - typedef VALUE4 X4; - typedef VALUE5 X5; - -protected: - - typedef NoiseModelFactor Base; - typedef NoiseModelFactor5 This; - -public: - - /** - * Default Constructor for I/O +/** @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(). + */ +template +class GTSAM_DEPRECATED NoiseModelFactor5 + : public NoiseModelFactorN { + public: + /** Aliases for value types pulled from keys. + * Note: in your code you can probably just do: + * `using X1 = ValueType<1>;` + * but this class is uglier due to dependent types. + * See e.g. testNonlinearFactor.cpp:TestFactorN. */ - NoiseModelFactor5() {} + using X1 = typename NoiseModelFactor5::template ValueType<1>; + using X2 = typename NoiseModelFactor5::template ValueType<2>; + using X3 = typename NoiseModelFactor5::template ValueType<3>; + using X4 = typename NoiseModelFactor5::template ValueType<4>; + using X5 = typename NoiseModelFactor5::template ValueType<5>; - /** - * Constructor - * @param noiseModel shared pointer to noise model - * @param j1 key of the first variable - * @param j2 key of the second variable - * @param j3 key of the third variable - * @param j4 key of the fourth variable - * @param j5 key of the fifth variable - */ - NoiseModelFactor5(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5) : - Base(noiseModel, cref_list_of<5>(j1)(j2)(j3)(j4)(j5)) {} + protected: + using Base = NoiseModelFactor; + using This = NoiseModelFactor5; + public: + // inherit NoiseModelFactorN's constructors + using NoiseModelFactorN::NoiseModelFactorN; ~NoiseModelFactor5() override {} - /** methods to retrieve keys */ - inline Key key1() const { return keys_[0]; } - inline Key key2() const { return keys_[1]; } - inline Key key3() const { return keys_[2]; } - inline Key key4() const { return keys_[3]; } - inline Key key5() const { return keys_[4]; } - - /** Calls the 5-key specific version of evaluateError, which is pure virtual - * so must be implemented in the derived class. */ - Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const override { - if(this->active(x)) { - if(H) - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]); - else - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4])); - } else { - return Vector::Zero(this->dim()); - } - } - - /** - * Override this method to finish implementing a 5-way factor. - * If any of the optional Matrix reference arguments are specified, it should compute - * both the function evaluation and its derivative(s) in X1 (and/or X2, X3). - */ - virtual Vector - evaluateError(const X1&, const X2&, const X3&, const X4&, const X5&, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none, - boost::optional H5 = boost::none) const = 0; - -private: - + private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("NoiseModelFactor", - boost::serialization::base_object(*this)); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor", boost::serialization::base_object(*this)); } -}; // \class NoiseModelFactor5 +}; // \class NoiseModelFactor5 /* ************************************************************************* */ -/** A convenient base class for creating your own NoiseModelFactor with 6 - * variables. To derive from this class, implement evaluateError(). */ -template -class NoiseModelFactor6: public NoiseModelFactor { - -public: - - // typedefs for value types pulled from keys - typedef VALUE1 X1; - typedef VALUE2 X2; - typedef VALUE3 X3; - typedef VALUE4 X4; - typedef VALUE5 X5; - typedef VALUE6 X6; - -protected: - - typedef NoiseModelFactor Base; - typedef NoiseModelFactor6 This; - -public: - - /** - * Default Constructor for I/O +/** @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(). + */ +template +class GTSAM_DEPRECATED NoiseModelFactor6 + : public NoiseModelFactorN { + public: + /** Aliases for value types pulled from keys. + * Note: in your code you can probably just do: + * `using X1 = ValueType<1>;` + * but this class is uglier due to dependent types. + * See e.g. testNonlinearFactor.cpp:TestFactorN. */ - NoiseModelFactor6() {} + using X1 = typename NoiseModelFactor6::template ValueType<1>; + using X2 = typename NoiseModelFactor6::template ValueType<2>; + using X3 = typename NoiseModelFactor6::template ValueType<3>; + using X4 = typename NoiseModelFactor6::template ValueType<4>; + using X5 = typename NoiseModelFactor6::template ValueType<5>; + using X6 = typename NoiseModelFactor6::template ValueType<6>; - /** - * Constructor - * @param noiseModel shared pointer to noise model - * @param j1 key of the first variable - * @param j2 key of the second variable - * @param j3 key of the third variable - * @param j4 key of the fourth variable - * @param j5 key of the fifth variable - * @param j6 key of the fifth variable - */ - NoiseModelFactor6(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) : - Base(noiseModel, cref_list_of<6>(j1)(j2)(j3)(j4)(j5)(j6)) {} + protected: + using Base = NoiseModelFactor; + using This = + NoiseModelFactor6; + public: + // inherit NoiseModelFactorN's constructors + using NoiseModelFactorN::NoiseModelFactorN; ~NoiseModelFactor6() override {} - /** methods to retrieve keys */ - inline Key key1() const { return keys_[0]; } - inline Key key2() const { return keys_[1]; } - inline Key key3() const { return keys_[2]; } - inline Key key4() const { return keys_[3]; } - inline Key key5() const { return keys_[4]; } - inline Key key6() const { return keys_[5]; } - - /** Calls the 6-key specific version of evaluateError, which is pure virtual - * so must be implemented in the derived class. */ - Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const override { - if(this->active(x)) { - if(H) - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4]), x.at(keys_[5]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]); - else - return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4]), x.at(keys_[5])); - } else { - return Vector::Zero(this->dim()); - } - } - - /** - * Override this method to finish implementing a 6-way factor. - * If any of the optional Matrix reference arguments are specified, it should compute - * both the function evaluation and its derivative(s) in X1 (and/or X2, X3). - */ - virtual Vector - evaluateError(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none, - boost::optional H5 = boost::none, - boost::optional H6 = boost::none) const = 0; - -private: - + private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("NoiseModelFactor", - boost::serialization::base_object(*this)); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor", boost::serialization::base_object(*this)); } -}; // \class NoiseModelFactor6 - -/* ************************************************************************* */ +}; // \class NoiseModelFactor6 } // \namespace gtsam diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h index 2d4a0ca32..d81ffbd31 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/nonlinear/tests/priorFactor.xml b/gtsam/nonlinear/tests/priorFactor.xml new file mode 100644 index 000000000..0c580fb21 --- /dev/null +++ b/gtsam/nonlinear/tests/priorFactor.xml @@ -0,0 +1,77 @@ + + + + + + + + + 1 + 0 + 12345 + + + + + + + + + 6 + + + 0 + + + + 6 + + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + + + + 6 + + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + + + + 1.00000000000000000e+00 + 1.00000000000000000e+00 + + + + + + + + 4.11982245665682978e-01 + -8.33737651774156818e-01 + -3.67630462924899259e-01 + -5.87266449276209815e-02 + -4.26917621276207360e-01 + 9.02381585483330806e-01 + -9.09297426825681709e-01 + -3.50175488374014632e-01 + -2.24845095366152908e-01 + + + + 4.00000000000000000e+00 + 5.00000000000000000e+00 + 6.00000000000000000e+00 + + + + + diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index 4a73cbb0b..f402656c1 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -107,8 +107,60 @@ TEST (Serialization, TemplatedValues) { EXPECT(equalsBinary(values)); } -TEST(Serialization, ISAM2) { +/** + * Test deserializing from a known serialization generated by code from commit + * 0af17f438f62f4788f3a572ecd36d06d224fd1e1 (>4.2a7) + * We only test that deserialization matches since + * (1) that's the main backward compatibility requirement and + * (2) serialized string depends on boost version + * Also note: we don't run this test when quaternions or TBB are enabled since + * serialization structures are different and the serialized strings/xml are + * hard-coded in this test. + */ +TEST(Serialization, NoiseModelFactor1_backwards_compatibility) { + PriorFactor factor( + 12345, Pose3(Rot3::RzRyRx(Vector3(1., 2., 3.)), Point3(4., 5., 6.)), + noiseModel::Unit::Create(6)); + // roundtrip (sanity check) + PriorFactor factor_deserialized_str_2 = PriorFactor(); + roundtrip(factor, factor_deserialized_str_2); + EXPECT(assert_equal(factor, factor_deserialized_str_2)); + +#if !defined(GTSAM_USE_QUATERNIONS) && !defined(GTSAM_USE_TBB) + // Deserialize string + std::string serialized_str = + "22 serialization::archive 15 1 0\n" + "0 0 0 0 0 0 0 1 0 12345 0 1 6 21 gtsam_noiseModel_Unit 1 0\n" + "1 1 0\n" + "2 1 0\n" + "3 0 0 0 0 6 0 1 0 0 0 6 1.00000000000000000e+00 1.00000000000000000e+00 " + "1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 " + "1.00000000000000000e+00 6 1.00000000000000000e+00 " + "1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 " + "1.00000000000000000e+00 1.00000000000000000e+00 1.00000000000000000e+00 " + "1.00000000000000000e+00 0 0 0 0 4.11982245665682978e-01 " + "-8.33737651774156818e-01 -3.67630462924899259e-01 " + "-5.87266449276209815e-02 -4.26917621276207360e-01 " + "9.02381585483330806e-01 -9.09297426825681709e-01 " + "-3.50175488374014632e-01 -2.24845095366152908e-01 0 0 " + "4.00000000000000000e+00 5.00000000000000000e+00 " + "6.00000000000000000e+00\n"; + + PriorFactor factor_deserialized_str = PriorFactor(); + deserializeFromString(serialized_str, factor_deserialized_str); + EXPECT(assert_equal(factor, factor_deserialized_str)); + + // Deserialize XML + PriorFactor factor_deserialized_xml = PriorFactor(); + deserializeFromXMLFile(GTSAM_SOURCE_TREE_DATASET_DIR + "/../../gtsam/nonlinear/tests/priorFactor.xml", + factor_deserialized_xml); + EXPECT(assert_equal(factor, factor_deserialized_xml)); +#endif +} + +TEST(Serialization, ISAM2) { gtsam::ISAM2Params parameters; gtsam::ISAM2 solver(parameters); gtsam::NonlinearFactorGraph graph; diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 58e98ebfa..c00669e36 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 b911fb5a4..a48b6e6fa 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 3c43c2c52..78cc39765 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 @@ public: 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 735746b3a..8850d7d2d 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 dc72f07b2..f2b41e0a1 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 c09d4136d..79890ec94 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 @@ private: 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 @@ private: * 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 @@ private: 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 5397c2e57..c1f8b286b 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 dd3c52286..9d84dfa7b 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 @@ private: 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/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 5997ad224..8a0277a45 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 05e23ce6d..60f880a7a 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 23f10de34..f3089bd71 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 @@ protected: 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 @@ public: // 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 @@ private: 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 @@ public: 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 @@ public: 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 @@ private: 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 43404df53..6bb1b0f36 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 d7508f6b8..7ead4ad11 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 81bb790de..1550201ec 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 20f12dbce..759e66341 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 @@ private: 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 5bb3745e9..e009ace29 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 @@ private: 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 0fc1c0b54..add6c86c4 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 40c8e29b7..74e844404 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 @@ public: 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 9e4091111..85539e17e 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 @@ public: * 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 ca1774e31..3be255e45 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 @@ private: 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 @@ public: 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 @@ private: 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 80d323019..f33ba2ca2 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 @@ public: 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 511cbd839..f25d59ab7 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 9f00f81d6..dcca22695 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 9a742b4f0..4eaa3139d 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 1e0ca621c..d7301a576 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 @@ public: * - 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 @@ public: * \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 @@ public: * \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 1fb895833..941b7c7ac 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 @@ public: /** * 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 d24d06e79..0fa3d9cb7 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 c9f05cf98..2880b9c8c 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 @@ private: 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 250c15b83..6f0aa605b 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 cabbfdbe8..43001fbc2 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 @@ public: /** print */ void print(const std::string& s = "EquivInertialNavFactor_GlobalVel", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "(" - << keyFormatter(this->key1()) << "," - << 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 b053b13f8..6423fabda 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 @@ public: const std::string& s = "EquivInertialNavFactor_GlobalVel_NoBias", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "(" - << keyFormatter(this->key1()) << "," - << 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 52e4f44cb..c3682e536 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 @@ public: /** 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 0828fbd08..efaf71d22 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 @@ public: /** print */ void print(const std::string& s = "InertialNavFactor_GlobalVelocity", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "(" - << keyFormatter(this->key1()) << "," - << 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 44d3b8fd0..59a834f0b 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 @@ protected: public: /// shorthand for base class type - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; /// shorthand for this class typedef InvDepthFactor3 This; @@ -93,8 +93,8 @@ public: 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 40c09416c..a41a06ccd 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 @@ protected: public: /// shorthand for base class type - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; /// shorthand for this class typedef InvDepthFactorVariant1 This; @@ -93,8 +93,8 @@ public: 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 b1169580e..d120eff32 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 @@ protected: public: /// shorthand for base class type - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; /// shorthand for this class typedef InvDepthFactorVariant2 This; @@ -96,8 +96,8 @@ public: 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 98f2db2f3..cade280f0 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 @@ protected: public: /// shorthand for base class type - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactorN Base; /// shorthand for this class typedef InvDepthFactorVariant3a This; @@ -96,8 +96,8 @@ public: 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 @@ private: /** * 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 @@ protected: public: /// shorthand for base class type - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactorN Base; /// shorthand for this class typedef InvDepthFactorVariant3b This; @@ -222,8 +222,8 @@ public: 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 f81c18bfa..f2c5dd3a9 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 91d79f208..34ab51d15 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 52a57b945..b49b49131 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 b47dcaf33..676e011de 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 b0cb25cfa..c7a094bda 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 8d183015e..ad1ba5fbe 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 d6f643c6c..8962b31b2 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 5c9a8c691..afbf85838 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 c92a13daf..8173c9dbd 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 597d894da..806f54fa5 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 ebf91d1f7..c6273ff4b 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 @@ private: 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/TSAMFactors.h b/gtsam_unstable/slam/TSAMFactors.h index 6f98a2dbd..2e024c5bb 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 @@ public: /** * 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 @@ public: /** * 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 3afa83f45..afe731bd5 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 097dbd3fe..2cd0d0f39 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 924a5fe4e..086da7cad 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 3b4afb106..4a20acd15 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 ca9a8580f..38b202c41 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 f0c1b4b70..88da7006d 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 @@ public: /* 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 @@ public: /** 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 @@ public: * 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 @@ public: 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 @@ public: }; // 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 @@ public: * 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 67a23355d..c536a48c3 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -330,10 +330,76 @@ 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, + "This type wrong"); + + public: + typedef NoiseModelFactor1 Base; + TestFactor1() : Base(noiseModel::Diagonal::Sigmas(Vector1(2.0)), L(1)) {} + using Base::NoiseModelFactor1; // inherit constructors + + Vector evaluateError(const double& x1, boost::optional H1 = + boost::none) const override { + if (H1) *H1 = (Matrix(1, 1) << 1.0).finished(); + return (Vector(1) << x1).finished(); + } + + gtsam::NonlinearFactor::shared_ptr clone() const override { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new TestFactor1(*this))); + } +}; +DIAGNOSTIC_POP() + +/* ************************************ */ +TEST(NonlinearFactor, NoiseModelFactor1) { + TestFactor1 tf; + Values tv; + tv.insert(L(1), double((1.0))); + EXPECT(assert_equal((Vector(1) << 1.0).finished(), tf.unwhitenedError(tv))); + DOUBLES_EQUAL(0.25 / 2.0, tf.error(tv), 1e-9); + JacobianFactor jf( + *boost::dynamic_pointer_cast(tf.linearize(tv))); + LONGS_EQUAL((long)L(1), (long)jf.keys()[0]); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 0.5).finished(), + jf.getA(jf.begin()))); + EXPECT(assert_equal((Vector)(Vector(1) << -0.5).finished(), jf.getb())); + + // Test all functions/types for backwards compatibility + static_assert(std::is_same::value, + "X type incorrect"); + EXPECT(assert_equal(tf.key(), L(1))); + std::vector H = {Matrix()}; + EXPECT(assert_equal(Vector1(1.0), tf.unwhitenedError(tv, H))); + + // Test constructors + TestFactor1 tf2(noiseModel::Unit::Create(1), L(1)); + TestFactor1 tf3(noiseModel::Unit::Create(1), {L(1)}); + TestFactor1 tf4(noiseModel::Unit::Create(1), gtsam::Symbol('L', 1)); +} + +/* ************************************************************************* */ +IGNORE_DEPRECATED_PUSH class TestFactor4 : public NoiseModelFactor4 { -public: + static_assert(std::is_same::value, "Base type wrong"); + static_assert( + std::is_same>::value, + "This type wrong"); + + public: typedef NoiseModelFactor4 Base; TestFactor4() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4)) {} + using Base::NoiseModelFactor4; // inherit constructors Vector evaluateError(const double& x1, const double& x2, const double& x3, const double& x4, @@ -347,13 +413,14 @@ public: *H3 = (Matrix(1, 1) << 3.0).finished(); *H4 = (Matrix(1, 1) << 4.0).finished(); } - return (Vector(1) << x1 + x2 + x3 + x4).finished(); + return (Vector(1) << x1 + 2.0 * x2 + 3.0 * x3 + 4.0 * x4).finished(); } gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new TestFactor4(*this))); } }; +DIAGNOSTIC_POP() /* ************************************ */ TEST(NonlinearFactor, NoiseModelFactor4) { @@ -363,8 +430,8 @@ TEST(NonlinearFactor, NoiseModelFactor4) { tv.insert(X(2), double((2.0))); tv.insert(X(3), double((3.0))); tv.insert(X(4), double((4.0))); - EXPECT(assert_equal((Vector(1) << 10.0).finished(), tf.unwhitenedError(tv))); - DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9); + EXPECT(assert_equal((Vector(1) << 30.0).finished(), tf.unwhitenedError(tv))); + DOUBLES_EQUAL(0.5 * 30.0 * 30.0 / 4.0, tf.error(tv), 1e-9); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); LONGS_EQUAL((long)X(1), (long)jf.keys()[0]); LONGS_EQUAL((long)X(2), (long)jf.keys()[1]); @@ -374,10 +441,55 @@ TEST(NonlinearFactor, NoiseModelFactor4) { EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.0).finished(), jf.getA(jf.begin()+1))); EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5).finished(), jf.getA(jf.begin()+2))); EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0).finished(), jf.getA(jf.begin()+3))); - EXPECT(assert_equal((Vector)(Vector(1) << -5.0).finished(), jf.getb())); + 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, + "X2 type incorrect"); + static_assert(std::is_same::value, + "X3 type incorrect"); + static_assert(std::is_same::value, + "X4 type incorrect"); + EXPECT(assert_equal(tf.key1(), X(1))); + EXPECT(assert_equal(tf.key2(), X(2))); + EXPECT(assert_equal(tf.key3(), X(3))); + EXPECT(assert_equal(tf.key4(), X(4))); + std::vector H = {Matrix(), Matrix(), Matrix(), Matrix()}; + EXPECT(assert_equal(Vector1(30.0), tf.unwhitenedError(tv, H))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.).finished(), H.at(0))); + 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, + "ValueType<1> type incorrect"); + static_assert(std::is_same, double>::value, + "ValueType<2> type incorrect"); + static_assert(std::is_same, double>::value, + "ValueType<3> type incorrect"); + static_assert(std::is_same, double>::value, + "ValueType<4> type incorrect"); + EXPECT(assert_equal(tf.key<1>(), X(1))); + EXPECT(assert_equal(tf.key<2>(), X(2))); + EXPECT(assert_equal(tf.key<3>(), X(3))); + EXPECT(assert_equal(tf.key<4>(), X(4))); + + // Test constructors + TestFactor4 tf2(noiseModel::Unit::Create(1), L(1), L(2), L(3), L(4)); + TestFactor4 tf3(noiseModel::Unit::Create(1), {L(1), L(2), L(3), L(4)}); + TestFactor4 tf4(noiseModel::Unit::Create(1), + std::array{L(1), L(2), L(3), L(4)}); + std::vector keys = {L(1), L(2), L(3), L(4)}; + TestFactor4 tf5(noiseModel::Unit::Create(1), keys); } /* ************************************************************************* */ +IGNORE_DEPRECATED_PUSH class TestFactor5 : public NoiseModelFactor5 { public: typedef NoiseModelFactor5 Base; @@ -397,9 +509,11 @@ public: *H4 = (Matrix(1, 1) << 4.0).finished(); *H5 = (Matrix(1, 1) << 5.0).finished(); } - return (Vector(1) << x1 + x2 + x3 + x4 + x5).finished(); + return (Vector(1) << x1 + 2.0 * x2 + 3.0 * x3 + 4.0 * x4 + 5.0 * x5) + .finished(); } }; +DIAGNOSTIC_POP() /* ************************************ */ TEST(NonlinearFactor, NoiseModelFactor5) { @@ -410,8 +524,8 @@ TEST(NonlinearFactor, NoiseModelFactor5) { tv.insert(X(3), double((3.0))); tv.insert(X(4), double((4.0))); tv.insert(X(5), double((5.0))); - EXPECT(assert_equal((Vector(1) << 15.0).finished(), tf.unwhitenedError(tv))); - DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9); + EXPECT(assert_equal((Vector(1) << 55.0).finished(), tf.unwhitenedError(tv))); + DOUBLES_EQUAL(0.5 * 55.0 * 55.0 / 4.0, tf.error(tv), 1e-9); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); LONGS_EQUAL((long)X(1), (long)jf.keys()[0]); LONGS_EQUAL((long)X(2), (long)jf.keys()[1]); @@ -423,10 +537,11 @@ TEST(NonlinearFactor, NoiseModelFactor5) { EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5).finished(), jf.getA(jf.begin()+2))); EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0).finished(), jf.getA(jf.begin()+3))); EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.5).finished(), jf.getA(jf.begin()+4))); - EXPECT(assert_equal((Vector)(Vector(1) << -7.5).finished(), jf.getb())); + EXPECT(assert_equal((Vector)(Vector(1) << 0.5 * -55.).finished(), jf.getb())); } /* ************************************************************************* */ +IGNORE_DEPRECATED_PUSH class TestFactor6 : public NoiseModelFactor6 { public: typedef NoiseModelFactor6 Base; @@ -448,10 +563,13 @@ public: *H5 = (Matrix(1, 1) << 5.0).finished(); *H6 = (Matrix(1, 1) << 6.0).finished(); } - return (Vector(1) << x1 + x2 + x3 + x4 + x5 + x6).finished(); + return (Vector(1) << x1 + 2.0 * x2 + 3.0 * x3 + 4.0 * x4 + 5.0 * x5 + + 6.0 * x6) + .finished(); } }; +DIAGNOSTIC_POP() /* ************************************ */ TEST(NonlinearFactor, NoiseModelFactor6) { @@ -463,8 +581,8 @@ TEST(NonlinearFactor, NoiseModelFactor6) { tv.insert(X(4), double((4.0))); tv.insert(X(5), double((5.0))); tv.insert(X(6), double((6.0))); - EXPECT(assert_equal((Vector(1) << 21.0).finished(), tf.unwhitenedError(tv))); - DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9); + EXPECT(assert_equal((Vector(1) << 91.0).finished(), tf.unwhitenedError(tv))); + DOUBLES_EQUAL(0.5 * 91.0 * 91.0 / 4.0, tf.error(tv), 1e-9); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); LONGS_EQUAL((long)X(1), (long)jf.keys()[0]); LONGS_EQUAL((long)X(2), (long)jf.keys()[1]); @@ -478,10 +596,107 @@ TEST(NonlinearFactor, NoiseModelFactor6) { EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0).finished(), jf.getA(jf.begin()+3))); EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.5).finished(), jf.getA(jf.begin()+4))); EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 3.0).finished(), jf.getA(jf.begin()+5))); - EXPECT(assert_equal((Vector)(Vector(1) << -10.5).finished(), jf.getb())); + EXPECT(assert_equal((Vector)(Vector(1) << 0.5 * -91.).finished(), jf.getb())); } +/* ************************************************************************* */ +class TestFactorN : public NoiseModelFactorN { +public: + typedef NoiseModelFactorN Base; + using Type1 = ValueType<1>; // Test that we can use the ValueType<> template + + TestFactorN() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4)) {} + + Vector + evaluateError(const double& x1, const double& x2, const double& x3, const double& x4, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none, + boost::optional H4 = boost::none) const override { + if (H1) *H1 = (Matrix(1, 1) << 1.0).finished(); + if (H2) *H2 = (Matrix(1, 1) << 2.0).finished(); + if (H3) *H3 = (Matrix(1, 1) << 3.0).finished(); + if (H4) *H4 = (Matrix(1, 1) << 4.0).finished(); + return (Vector(1) << x1 + 2.0 * x2 + 3.0 * x3 + 4.0 * x4).finished(); + } + + Key key1() const { return key<1>(); } // Test that we can use key<> template +}; + +/* ************************************ */ +TEST(NonlinearFactor, NoiseModelFactorN) { + TestFactorN tf; + Values tv; + tv.insert(X(1), double((1.0))); + tv.insert(X(2), double((2.0))); + tv.insert(X(3), double((3.0))); + tv.insert(X(4), double((4.0))); + EXPECT(assert_equal((Vector(1) << 30.0).finished(), tf.unwhitenedError(tv))); + DOUBLES_EQUAL(0.5 * 30.0 * 30.0 / 4.0, tf.error(tv), 1e-9); + JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); + LONGS_EQUAL((long)X(1), (long)jf.keys()[0]); + LONGS_EQUAL((long)X(2), (long)jf.keys()[1]); + LONGS_EQUAL((long)X(3), (long)jf.keys()[2]); + LONGS_EQUAL((long)X(4), (long)jf.keys()[3]); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 0.5).finished(), jf.getA(jf.begin()))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.0).finished(), jf.getA(jf.begin()+1))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 1.5).finished(), jf.getA(jf.begin()+2))); + EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 2.0).finished(), jf.getA(jf.begin()+3))); + EXPECT(assert_equal((Vector)(Vector(1) << -0.5 * 30.).finished(), jf.getb())); + + // Test all evaluateError argument overloads to ensure backward compatibility + Matrix H1_expected, H2_expected, H3_expected, H4_expected; + Vector e_expected = tf.evaluateError(9, 8, 7, 6, H1_expected, H2_expected, + H3_expected, H4_expected); + + std::unique_ptr> base_ptr( + new TestFactorN(tf)); + Matrix H1, H2, H3, H4; + EXPECT(assert_equal(e_expected, base_ptr->evaluateError(9, 8, 7, 6))); + EXPECT(assert_equal(e_expected, base_ptr->evaluateError(9, 8, 7, 6, H1))); + EXPECT(assert_equal(H1_expected, H1)); + EXPECT(assert_equal(e_expected, // + base_ptr->evaluateError(9, 8, 7, 6, H1, H2))); + EXPECT(assert_equal(H1_expected, H1)); + EXPECT(assert_equal(H2_expected, H2)); + EXPECT(assert_equal(e_expected, + base_ptr->evaluateError(9, 8, 7, 6, H1, H2, H3))); + EXPECT(assert_equal(H1_expected, H1)); + EXPECT(assert_equal(H2_expected, H2)); + EXPECT(assert_equal(H3_expected, H3)); + EXPECT(assert_equal(e_expected, + base_ptr->evaluateError(9, 8, 7, 6, H1, H2, H3, H4))); + EXPECT(assert_equal(H1_expected, H1)); + EXPECT(assert_equal(H2_expected, H2)); + EXPECT(assert_equal(H3_expected, H3)); + EXPECT(assert_equal(H4_expected, H4)); + + // Test all functions/types for backwards compatibility + IGNORE_DEPRECATED_PUSH + static_assert(std::is_same::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"); + static_assert(std::is_same, double>::value, + "ValueType<2> type incorrect"); + static_assert(std::is_same, double>::value, + "ValueType<3> type incorrect"); + static_assert(std::is_same, double>::value, + "ValueType<4> type incorrect"); + static_assert(std::is_same::value, + "TestFactorN::Type1 type incorrect"); + EXPECT(assert_equal(tf.key<1>(), X(1))); + EXPECT(assert_equal(tf.key<2>(), X(2))); + EXPECT(assert_equal(tf.key<3>(), X(3))); + EXPECT(assert_equal(tf.key<4>(), X(4))); + EXPECT(assert_equal(tf.key1(), X(1))); +} + /* ************************************************************************* */ TEST( NonlinearFactor, clone_rekey ) { diff --git a/timing/timeIncremental.cpp b/timing/timeIncremental.cpp index 5e3fc9189..2b5ec474d 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 a056bde24..dcb42f763 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[]) {