diff --git a/gtsam/nonlinear/GaussNewtonOptimizer.h b/gtsam/nonlinear/GaussNewtonOptimizer.h index a436597e58..a3923524b7 100644 --- a/gtsam/nonlinear/GaussNewtonOptimizer.h +++ b/gtsam/nonlinear/GaussNewtonOptimizer.h @@ -28,6 +28,8 @@ class GaussNewtonOptimizer; * NonlinearOptimizationParams. */ class GTSAM_EXPORT GaussNewtonParams : public NonlinearOptimizerParams { +public: + using OptimizerType = GaussNewtonOptimizer; }; /** diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h new file mode 100644 index 0000000000..cd0b4c81a4 --- /dev/null +++ b/gtsam/nonlinear/GncOptimizer.h @@ -0,0 +1,338 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file GncOptimizer.h + * @brief The GncOptimizer class + * @author Jingnan Shi + * @author Luca Carlone + * @author Frank Dellaert + * + * Implementation of the paper: Yang, Antonante, Tzoumas, Carlone, "Graduated Non-Convexity for Robust Spatial Perception: + * From Non-Minimal Solvers to Global Outlier Rejection", ICRA/RAL, 2020. (arxiv version: https://arxiv.org/pdf/1909.08605.pdf) + * + * See also: + * Antonante, Tzoumas, Yang, Carlone, "Outlier-Robust Estimation: Hardness, Minimally-Tuned Algorithms, and Applications", + * arxiv: https://arxiv.org/pdf/2007.15109.pdf, 2020. + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +template +class GncOptimizer { + public: + /// For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer. + typedef typename GncParameters::OptimizerType BaseOptimizer; + + private: + NonlinearFactorGraph nfg_; ///< Original factor graph to be solved by GNC. + Values state_; ///< Initial values to be used at each iteration by GNC. + GncParameters params_; ///< GNC parameters. + Vector weights_; ///< Weights associated to each factor in GNC (this could be a local variable in optimize, but it is useful to make it accessible from outside). + + public: + /// Constructor. + GncOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, + const GncParameters& params = GncParameters()) + : state_(initialValues), + params_(params) { + + // make sure all noiseModels are Gaussian or convert to Gaussian + nfg_.resize(graph.size()); + for (size_t i = 0; i < graph.size(); i++) { + if (graph[i]) { + NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast< + NoiseModelFactor>(graph[i]); + auto robust = boost::dynamic_pointer_cast< + noiseModel::Robust>(factor->noiseModel()); + // if the factor has a robust loss, we remove the robust loss + nfg_[i] = robust ? factor-> cloneWithNewNoiseModel(robust->noise()) : factor; + } + } + } + + /// Access a copy of the internal factor graph. + const NonlinearFactorGraph& getFactors() const { return nfg_; } + + /// Access a copy of the internal values. + const Values& getState() const { return state_; } + + /// Access a copy of the parameters. + const GncParameters& getParams() const { return params_;} + + /// Access a copy of the GNC weights. + const Vector& getWeights() const { return weights_;} + + /// Compute optimal solution using graduated non-convexity. + Values optimize() { + // start by assuming all measurements are inliers + weights_ = Vector::Ones(nfg_.size()); + BaseOptimizer baseOptimizer(nfg_, state_); + Values result = baseOptimizer.optimize(); + double mu = initializeMu(); + double prev_cost = nfg_.error(result); + double cost = 0.0; // this will be updated in the main loop + + // handle the degenerate case that corresponds to small + // maximum residual errors at initialization + // For GM: if residual error is small, mu -> 0 + // For TLS: if residual error is small, mu -> -1 + if (mu <= 0) { + if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) { + std::cout << "GNC Optimizer stopped because maximum residual at " + "initialization is small." + << std::endl; + } + if (params_.verbosity >= GncParameters::Verbosity::VALUES) { + result.print("result\n"); + std::cout << "mu: " << mu << std::endl; + } + return result; + } + + size_t iter; + for (iter = 0; iter < params_.maxIterations; iter++) { + + // display info + if (params_.verbosity >= GncParameters::Verbosity::VALUES) { + std::cout << "iter: " << iter << std::endl; + result.print("result\n"); + std::cout << "mu: " << mu << std::endl; + std::cout << "weights: " << weights_ << std::endl; + } + // weights update + weights_ = calculateWeights(result, mu); + + // variable/values update + NonlinearFactorGraph graph_iter = this->makeWeightedGraph(weights_); + BaseOptimizer baseOptimizer_iter(graph_iter, state_); + result = baseOptimizer_iter.optimize(); + + // stopping condition + cost = graph_iter.error(result); + if (checkConvergence(mu, weights_, cost, prev_cost)) { + break; + } + + // update mu + mu = updateMu(mu); + + // get ready for next iteration + prev_cost = cost; + + // display info + if (params_.verbosity >= GncParameters::Verbosity::VALUES) { + std::cout << "previous cost: " << prev_cost << std::endl; + std::cout << "current cost: " << cost << std::endl; + } + } + // display info + if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) { + std::cout << "final iterations: " << iter << std::endl; + std::cout << "final mu: " << mu << std::endl; + std::cout << "final weights: " << weights_ << std::endl; + std::cout << "previous cost: " << prev_cost << std::endl; + std::cout << "current cost: " << cost << std::endl; + } + return result; + } + + /// Initialize the gnc parameter mu such that loss is approximately convex (remark 5 in GNC paper). + double initializeMu() const { + // compute largest error across all factors + double rmax_sq = 0.0; + for (size_t i = 0; i < nfg_.size(); i++) { + if (nfg_[i]) { + rmax_sq = std::max(rmax_sq, nfg_[i]->error(state_)); + } + } + // set initial mu + switch (params_.lossType) { + case GncLossType::GM: + // surrogate cost is convex for large mu + return 2 * rmax_sq / params_.barcSq; // initial mu + case GncLossType::TLS: + /* initialize mu to the value specified in Remark 5 in GNC paper. + surrogate cost is convex for mu close to zero + degenerate case: 2 * rmax_sq - params_.barcSq < 0 (handled in the main loop) + according to remark mu = params_.barcSq / (2 * rmax_sq - params_.barcSq) = params_.barcSq/ excessResidual + however, if the denominator is 0 or negative, we return mu = -1 which leads to termination of the main GNC loop + */ + return + (2 * rmax_sq - params_.barcSq) > 0 ? + params_.barcSq / (2 * rmax_sq - params_.barcSq) : -1; + default: + throw std::runtime_error( + "GncOptimizer::initializeMu: called with unknown loss type."); + } + } + + /// Update the gnc parameter mu to gradually increase nonconvexity. + double updateMu(const double mu) const { + switch (params_.lossType) { + case GncLossType::GM: + // reduce mu, but saturate at 1 (original cost is recovered for mu -> 1) + return std::max(1.0, mu / params_.muStep); + case GncLossType::TLS: + // increases mu at each iteration (original cost is recovered for mu -> inf) + return mu * params_.muStep; + default: + throw std::runtime_error( + "GncOptimizer::updateMu: called with unknown loss type."); + } + } + + /// Check if we have reached the value of mu for which the surrogate loss matches the original loss. + bool checkMuConvergence(const double mu) const { + bool muConverged = false; + switch (params_.lossType) { + case GncLossType::GM: + muConverged = std::fabs(mu - 1.0) < 1e-9; // mu=1 recovers the original GM function + break; + case GncLossType::TLS: + muConverged = false; // for TLS there is no stopping condition on mu (it must tend to infinity) + break; + default: + throw std::runtime_error( + "GncOptimizer::checkMuConvergence: called with unknown loss type."); + } + if (muConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY) + std::cout << "muConverged = true " << std::endl; + return muConverged; + } + + /// Check convergence of relative cost differences. + bool checkCostConvergence(const double cost, const double prev_cost) const { + bool costConverged = std::fabs(cost - prev_cost) / std::max(prev_cost, 1e-7) + < params_.relativeCostTol; + if (costConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY) + std::cout << "checkCostConvergence = true " << std::endl; + return costConverged; + } + + /// Check convergence of weights to binary values. + bool checkWeightsConvergence(const Vector& weights) const { + bool weightsConverged = false; + switch (params_.lossType) { + case GncLossType::GM: + weightsConverged = false; // for GM, there is no clear binary convergence for the weights + break; + case GncLossType::TLS: + weightsConverged = true; + for (size_t i = 0; i < weights.size(); i++) { + if (std::fabs(weights[i] - std::round(weights[i])) + > params_.weightsTol) { + weightsConverged = false; + break; + } + } + break; + default: + throw std::runtime_error( + "GncOptimizer::checkWeightsConvergence: called with unknown loss type."); + } + if (weightsConverged + && params_.verbosity >= GncParameters::Verbosity::SUMMARY) + std::cout << "weightsConverged = true " << std::endl; + return weightsConverged; + } + + /// Check for convergence between consecutive GNC iterations. + bool checkConvergence(const double mu, const Vector& weights, + const double cost, const double prev_cost) const { + return checkCostConvergence(cost, prev_cost) + || checkWeightsConvergence(weights) || checkMuConvergence(mu); + } + + /// Create a graph where each factor is weighted by the gnc weights. + NonlinearFactorGraph makeWeightedGraph(const Vector& weights) const { + // make sure all noiseModels are Gaussian or convert to Gaussian + NonlinearFactorGraph newGraph; + newGraph.resize(nfg_.size()); + for (size_t i = 0; i < nfg_.size(); i++) { + if (nfg_[i]) { + auto factor = boost::dynamic_pointer_cast< + NoiseModelFactor>(nfg_[i]); + auto noiseModel = + boost::dynamic_pointer_cast( + factor->noiseModel()); + if (noiseModel) { + Matrix newInfo = weights[i] * noiseModel->information(); + auto newNoiseModel = noiseModel::Gaussian::Information(newInfo); + newGraph[i] = factor->cloneWithNewNoiseModel(newNoiseModel); + } else { + throw std::runtime_error( + "GncOptimizer::makeWeightedGraph: unexpected non-Gaussian noise model."); + } + } + } + return newGraph; + } + + /// Calculate gnc weights. + Vector calculateWeights(const Values& currentEstimate, const double mu) { + Vector weights = Vector::Ones(nfg_.size()); + + // do not update the weights that the user has decided are known inliers + std::vector allWeights; + for (size_t k = 0; k < nfg_.size(); k++) { + allWeights.push_back(k); + } + std::vector unknownWeights; + std::set_difference(allWeights.begin(), allWeights.end(), + params_.knownInliers.begin(), + params_.knownInliers.end(), + std::inserter(unknownWeights, unknownWeights.begin())); + + // update weights of known inlier/outlier measurements + switch (params_.lossType) { + case GncLossType::GM: { // use eq (12) in GNC paper + for (size_t k : unknownWeights) { + if (nfg_[k]) { + double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual + weights[k] = std::pow( + (mu * params_.barcSq) / (u2_k + mu * params_.barcSq), 2); + } + } + return weights; + } + case GncLossType::TLS: { // use eq (14) in GNC paper + double upperbound = (mu + 1) / mu * params_.barcSq; + double lowerbound = mu / (mu + 1) * params_.barcSq; + for (size_t k : unknownWeights) { + if (nfg_[k]) { + double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual + if (u2_k >= upperbound) { + weights[k] = 0; + } else if (u2_k <= lowerbound) { + weights[k] = 1; + } else { + weights[k] = std::sqrt(params_.barcSq * mu * (mu + 1) / u2_k) + - mu; + } + } + } + return weights; + } + default: + throw std::runtime_error( + "GncOptimizer::calculateWeights: called with unknown loss type."); + } + } +}; + +} diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h new file mode 100644 index 0000000000..0388a7fd16 --- /dev/null +++ b/gtsam/nonlinear/GncParams.h @@ -0,0 +1,164 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file GncOptimizer.h + * @brief The GncOptimizer class + * @author Jingnan Shi + * @author Luca Carlone + * @author Frank Dellaert + * + * Implementation of the paper: Yang, Antonante, Tzoumas, Carlone, "Graduated Non-Convexity for Robust Spatial Perception: + * From Non-Minimal Solvers to Global Outlier Rejection", ICRA/RAL, 2020. (arxiv version: https://arxiv.org/pdf/1909.08605.pdf) + * + * See also: + * Antonante, Tzoumas, Yang, Carlone, "Outlier-Robust Estimation: Hardness, Minimally-Tuned Algorithms, and Applications", + * arxiv: https://arxiv.org/pdf/2007.15109.pdf, 2020. + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +/// Choice of robust loss function for GNC. +enum GncLossType { + GM /*Geman McClure*/, + TLS /*Truncated least squares*/ +}; + +template +class GncParams { + public: + /// For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer. + typedef typename BaseOptimizerParameters::OptimizerType OptimizerType; + + /// Verbosity levels + enum Verbosity { + SILENT = 0, + SUMMARY, + VALUES + }; + + /// Constructor. + GncParams(const BaseOptimizerParameters& baseOptimizerParams) + : baseOptimizerParams(baseOptimizerParams) { + } + + /// Default constructor. + GncParams() + : baseOptimizerParams() { + } + + /// GNC parameters. + BaseOptimizerParameters baseOptimizerParams; ///< Optimization parameters used to solve the weighted least squares problem at each GNC iteration + /// any other specific GNC parameters: + GncLossType lossType = TLS; ///< Default loss + size_t maxIterations = 100; ///< Maximum number of iterations + double barcSq = 1.0; ///< A factor is considered an inlier if factor.error() < barcSq. Note that factor.error() whitens by the covariance + double muStep = 1.4; ///< Multiplicative factor to reduce/increase the mu in gnc + double relativeCostTol = 1e-5; ///< If relative cost change is below this threshold, stop iterating + double weightsTol = 1e-4; ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS) + Verbosity verbosity = SILENT; ///< Verbosity level + std::vector knownInliers = std::vector(); ///< Slots in the factor graph corresponding to measurements that we know are inliers + + /// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType). + void setLossType(const GncLossType type) { + lossType = type; + } + + /// Set the maximum number of iterations in GNC (changing the max nr of iters might lead to less accurate solutions and is not recommended). + void setMaxIterations(const size_t maxIter) { + std::cout + << "setMaxIterations: changing the max nr of iters might lead to less accurate solutions and is not recommended! " + << std::endl; + maxIterations = maxIter; + } + + /** Set the maximum weighted residual error for an inlier. For a factor in the form f(x) = 0.5 * || r(x) ||^2_Omega, + * the inlier threshold is the largest value of f(x) for the corresponding measurement to be considered an inlier. + * In other words, an inlier at x is such that 0.5 * || r(x) ||^2_Omega <= barcSq. + * Assuming a isotropic measurement covariance sigma^2 * Identity, the cost becomes: 0.5 * 1/sigma^2 || r(x) ||^2 <= barcSq. + * Hence || r(x) ||^2 <= 2 * barcSq * sigma^2. + * */ + void setInlierCostThreshold(const double inth) { + barcSq = inth; + } + + /// Set the graduated non-convexity step: at each GNC iteration, mu is updated as mu <- mu * muStep. + void setMuStep(const double step) { + muStep = step; + } + + /// Set the maximum relative difference in mu values to stop iterating. + void setRelativeCostTol(double value) { + relativeCostTol = value; + } + + /// Set the maximum difference between the weights and their rounding in {0,1} to stop iterating. + void setWeightsTol(double value) { + weightsTol = value; + } + + /// Set the verbosity level. + void setVerbosityGNC(const Verbosity value) { + verbosity = value; + } + + /** (Optional) Provide a vector of measurements that must be considered inliers. The enties in the vector + * corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg, + * and you provide knownIn = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15]. + * This functionality is commonly used in SLAM when one may assume the odometry is outlier free, and + * only apply GNC to prune outliers from the loop closures. + * */ + void setKnownInliers(const std::vector& knownIn) { + for (size_t i = 0; i < knownIn.size(); i++) + knownInliers.push_back(knownIn[i]); + } + + /// Equals. + bool equals(const GncParams& other, double tol = 1e-9) const { + return baseOptimizerParams.equals(other.baseOptimizerParams) + && lossType == other.lossType && maxIterations == other.maxIterations + && std::fabs(barcSq - other.barcSq) <= tol + && std::fabs(muStep - other.muStep) <= tol + && verbosity == other.verbosity && knownInliers == other.knownInliers; + } + + /// Print. + void print(const std::string& str) const { + std::cout << str << "\n"; + switch (lossType) { + case GM: + std::cout << "lossType: Geman McClure" << "\n"; + break; + case TLS: + std::cout << "lossType: Truncated Least-squares" << "\n"; + break; + default: + throw std::runtime_error("GncParams::print: unknown loss type."); + } + std::cout << "maxIterations: " << maxIterations << "\n"; + std::cout << "barcSq: " << barcSq << "\n"; + std::cout << "muStep: " << muStep << "\n"; + std::cout << "relativeCostTol: " << relativeCostTol << "\n"; + std::cout << "weightsTol: " << weightsTol << "\n"; + std::cout << "verbosity: " << verbosity << "\n"; + for (size_t i = 0; i < knownInliers.size(); i++) + std::cout << "knownInliers: " << knownInliers[i] << "\n"; + baseOptimizerParams.print(str); + } +}; + +} diff --git a/gtsam/nonlinear/LevenbergMarquardtParams.h b/gtsam/nonlinear/LevenbergMarquardtParams.h index 4962ad3660..30e783fc99 100644 --- a/gtsam/nonlinear/LevenbergMarquardtParams.h +++ b/gtsam/nonlinear/LevenbergMarquardtParams.h @@ -25,6 +25,8 @@ namespace gtsam { +class LevenbergMarquardtOptimizer; + /** Parameters for Levenberg-Marquardt optimization. Note that this parameters * class inherits from NonlinearOptimizerParams, which specifies the parameters * common to all nonlinear optimization algorithms. This class also contains @@ -40,6 +42,7 @@ class GTSAM_EXPORT LevenbergMarquardtParams: public NonlinearOptimizerParams { static VerbosityLM verbosityLMTranslator(const std::string &s); static std::string verbosityLMTranslator(VerbosityLM value); + using OptimizerType = LevenbergMarquardtOptimizer; public: diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index 1cfcba274b..8b8d2da6c2 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -76,6 +76,14 @@ bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const { && noiseModel_->equals(*e->noiseModel_, tol))); } +/* ************************************************************************* */ +NoiseModelFactor::shared_ptr NoiseModelFactor::cloneWithNewNoiseModel( + const SharedNoiseModel newNoise) const { + NoiseModelFactor::shared_ptr new_factor = boost::dynamic_pointer_cast(clone()); + new_factor->noiseModel_ = newNoise; + return new_factor; +} + /* ************************************************************************* */ static void check(const SharedNoiseModel& noiseModel, size_t m) { if (noiseModel && m != noiseModel->dim()) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 80fbfbb11c..00311fb87b 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -244,6 +244,12 @@ class GTSAM_EXPORT NoiseModelFactor: public NonlinearFactor { */ boost::shared_ptr linearize(const Values& x) const override; + /** + * Creates a shared_ptr clone of the + * factor with a new noise model + */ + shared_ptr cloneWithNewNoiseModel(const SharedNoiseModel newNoise) const; + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index a7bc10a1f4..218230421a 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -113,6 +113,17 @@ class GTSAM_EXPORT NonlinearOptimizerParams { virtual void print(const std::string& str = "") const; + bool equals(const NonlinearOptimizerParams& other, double tol = 1e-9) const { + return maxIterations == other.getMaxIterations() + && std::abs(relativeErrorTol - other.getRelativeErrorTol()) <= tol + && std::abs(absoluteErrorTol - other.getAbsoluteErrorTol()) <= tol + && std::abs(errorTol - other.getErrorTol()) <= tol + && verbosityTranslator(verbosity) == other.getVerbosity(); + // && orderingType.equals(other.getOrderingType()_; + // && linearSolverType == other.getLinearSolverType(); + // TODO: check ordering, iterativeParams, and iterationsHook + } + inline bool isMultifrontal() const { return (linearSolverType == MULTIFRONTAL_CHOLESKY) || (linearSolverType == MULTIFRONTAL_QR); diff --git a/tests/smallExample.h b/tests/smallExample.h index 0c933d1060..944899e701 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -342,10 +342,25 @@ struct UnaryFactor: public gtsam::NoiseModelFactor1 { return (h(x) - z_); } + gtsam::NonlinearFactor::shared_ptr clone() const override { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new UnaryFactor(*this))); } }; } +/* ************************************************************************* */ +inline NonlinearFactorGraph nonlinearFactorGraphWithGivenSigma(const double sigma) { + using symbol_shorthand::X; + using symbol_shorthand::L; + boost::shared_ptr fg(new NonlinearFactorGraph); + Point2 z(1.0, 0.0); + boost::shared_ptr factor( + new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1))); + fg->push_back(factor); + return *fg; +} + /* ************************************************************************* */ inline boost::shared_ptr sharedReallyNonlinearFactorGraph() { using symbol_shorthand::X; @@ -363,6 +378,54 @@ inline NonlinearFactorGraph createReallyNonlinearFactorGraph() { return *sharedReallyNonlinearFactorGraph(); } +/* ************************************************************************* */ +inline NonlinearFactorGraph sharedNonRobustFactorGraphWithOutliers() { + using symbol_shorthand::X; + boost::shared_ptr fg(new NonlinearFactorGraph); + Point2 z(0.0, 0.0); + double sigma = 0.1; + + boost::shared_ptr> factor( + new PriorFactor(X(1), z, noiseModel::Isotropic::Sigma(2,sigma))); + // 3 noiseless inliers + fg->push_back(factor); + fg->push_back(factor); + fg->push_back(factor); + + // 1 outlier + Point2 z_out(1.0, 0.0); + boost::shared_ptr> factor_out( + new PriorFactor(X(1), z_out, noiseModel::Isotropic::Sigma(2,sigma))); + fg->push_back(factor_out); + + return *fg; +} + +/* ************************************************************************* */ +inline NonlinearFactorGraph sharedRobustFactorGraphWithOutliers() { + using symbol_shorthand::X; + boost::shared_ptr fg(new NonlinearFactorGraph); + Point2 z(0.0, 0.0); + double sigma = 0.1; + auto gmNoise = noiseModel::Robust::Create( + noiseModel::mEstimator::GemanMcClure::Create(1.0), noiseModel::Isotropic::Sigma(2,sigma)); + boost::shared_ptr> factor( + new PriorFactor(X(1), z, gmNoise)); + // 3 noiseless inliers + fg->push_back(factor); + fg->push_back(factor); + fg->push_back(factor); + + // 1 outlier + Point2 z_out(1.0, 0.0); + boost::shared_ptr> factor_out( + new PriorFactor(X(1), z_out, gmNoise)); + fg->push_back(factor_out); + + return *fg; +} + + /* ************************************************************************* */ inline std::pair createNonlinearSmoother(int T) { using namespace impl; diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp new file mode 100644 index 0000000000..738c77936a --- /dev/null +++ b/tests/testGncOptimizer.cpp @@ -0,0 +1,640 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testGncOptimizer.cpp + * @brief Unit tests for GncOptimizer class + * @author Jingnan Shi + * @author Luca Carlone + * @author Frank Dellaert + * + * Implementation of the paper: Yang, Antonante, Tzoumas, Carlone, "Graduated + * Non-Convexity for Robust Spatial Perception: From Non-Minimal Solvers to + * Global Outlier Rejection", ICRA/RAL, 2020. (arxiv version: + * https://arxiv.org/pdf/1909.08605.pdf) + * + * See also: + * Antonante, Tzoumas, Yang, Carlone, "Outlier-Robust Estimation: Hardness, + * Minimally-Tuned Algorithms, and Applications", arxiv: + * https://arxiv.org/pdf/2007.15109.pdf, 2020. + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +using symbol_shorthand::L; +using symbol_shorthand::X; +static double tol = 1e-7; + +/* ************************************************************************* */ +TEST(GncOptimizer, gncParamsConstructor) { + // check params are correctly parsed + LevenbergMarquardtParams lmParams; + GncParams gncParams1(lmParams); + CHECK(lmParams.equals(gncParams1.baseOptimizerParams)); + + // check also default constructor + GncParams gncParams1b; + CHECK(lmParams.equals(gncParams1b.baseOptimizerParams)); + + // and check params become different if we change lmParams + lmParams.setVerbosity("DELTA"); + CHECK(!lmParams.equals(gncParams1.baseOptimizerParams)); + + // and same for GN + GaussNewtonParams gnParams; + GncParams gncParams2(gnParams); + CHECK(gnParams.equals(gncParams2.baseOptimizerParams)); + + // check default constructor + GncParams gncParams2b; + CHECK(gnParams.equals(gncParams2b.baseOptimizerParams)); + + // change something at the gncParams level + GncParams gncParams2c(gncParams2b); + gncParams2c.setLossType(GncLossType::GM); + CHECK(!gncParams2c.equals(gncParams2b.baseOptimizerParams)); +} + +/* ************************************************************************* */ +TEST(GncOptimizer, gncConstructor) { + // has to have Gaussian noise models ! + auto fg = example::createReallyNonlinearFactorGraph(); // just a unary factor + // on a 2D point + + Point2 p0(3, 3); + Values initial; + initial.insert(X(1), p0); + + GncParams gncParams; + auto gnc = GncOptimizer>(fg, initial, + gncParams); + + CHECK(gnc.getFactors().equals(fg)); + CHECK(gnc.getState().equals(initial)); + CHECK(gnc.getParams().equals(gncParams)); +} + +/* ************************************************************************* */ +TEST(GncOptimizer, gncConstructorWithRobustGraphAsInput) { + auto fg = example::sharedNonRobustFactorGraphWithOutliers(); + // same graph with robust noise model + auto fg_robust = example::sharedRobustFactorGraphWithOutliers(); + + Point2 p0(3, 3); + Values initial; + initial.insert(X(1), p0); + + GncParams gncParams; + auto gnc = GncOptimizer>(fg_robust, + initial, + gncParams); + + // make sure that when parsing the graph is transformed into one without + // robust loss + CHECK(fg.equals(gnc.getFactors())); +} + +/* ************************************************************************* */ +TEST(GncOptimizer, initializeMu) { + auto fg = example::createReallyNonlinearFactorGraph(); + + Point2 p0(3, 3); + Values initial; + initial.insert(X(1), p0); + + // testing GM mu initialization + GncParams gncParams; + gncParams.setLossType(GncLossType::GM); + auto gnc_gm = GncOptimizer>(fg, initial, + gncParams); + // according to rmk 5 in the gnc paper: m0 = 2 rmax^2 / barcSq + // (barcSq=1 in this example) + EXPECT_DOUBLES_EQUAL(gnc_gm.initializeMu(), 2 * 198.999, 1e-3); + + // testing TLS mu initialization + gncParams.setLossType(GncLossType::TLS); + auto gnc_tls = GncOptimizer>(fg, initial, + gncParams); + // according to rmk 5 in the gnc paper: m0 = barcSq / (2 * rmax^2 - barcSq) + // (barcSq=1 in this example) + EXPECT_DOUBLES_EQUAL(gnc_tls.initializeMu(), 1 / (2 * 198.999 - 1), 1e-3); +} + +/* ************************************************************************* */ +TEST(GncOptimizer, updateMuGM) { + // has to have Gaussian noise models ! + auto fg = example::createReallyNonlinearFactorGraph(); + + Point2 p0(3, 3); + Values initial; + initial.insert(X(1), p0); + + GncParams gncParams; + gncParams.setLossType(GncLossType::GM); + gncParams.setMuStep(1.4); + auto gnc = GncOptimizer>(fg, initial, + gncParams); + + double mu = 5.0; + EXPECT_DOUBLES_EQUAL(gnc.updateMu(mu), mu / 1.4, tol); + + // check it correctly saturates to 1 for GM + mu = 1.2; + EXPECT_DOUBLES_EQUAL(gnc.updateMu(mu), 1.0, tol); +} + +/* ************************************************************************* */ +TEST(GncOptimizer, updateMuTLS) { + // has to have Gaussian noise models ! + auto fg = example::createReallyNonlinearFactorGraph(); + + Point2 p0(3, 3); + Values initial; + initial.insert(X(1), p0); + + GncParams gncParams; + gncParams.setMuStep(1.4); + gncParams.setLossType(GncLossType::TLS); + auto gnc = GncOptimizer>(fg, initial, + gncParams); + + double mu = 5.0; + EXPECT_DOUBLES_EQUAL(gnc.updateMu(mu), mu * 1.4, tol); +} + +/* ************************************************************************* */ +TEST(GncOptimizer, checkMuConvergence) { + // has to have Gaussian noise models ! + auto fg = example::createReallyNonlinearFactorGraph(); + + Point2 p0(3, 3); + Values initial; + initial.insert(X(1), p0); + + { + GncParams gncParams; + gncParams.setLossType(GncLossType::GM); + auto gnc = GncOptimizer>(fg, initial, + gncParams); + + double mu = 1.0; + CHECK(gnc.checkMuConvergence(mu)); + } + { + GncParams gncParams; + gncParams.setLossType( + GncLossType::TLS); + auto gnc = GncOptimizer>(fg, initial, + gncParams); + + double mu = 1.0; + CHECK(!gnc.checkMuConvergence(mu)); //always false for TLS + } +} + +/* ************************************************************************* */ +TEST(GncOptimizer, checkCostConvergence) { + // has to have Gaussian noise models ! + auto fg = example::createReallyNonlinearFactorGraph(); + + Point2 p0(3, 3); + Values initial; + initial.insert(X(1), p0); + + { + GncParams gncParams; + gncParams.setRelativeCostTol(0.49); + auto gnc = GncOptimizer>(fg, initial, + gncParams); + + double prev_cost = 1.0; + double cost = 0.5; + // relative cost reduction = 0.5 > 0.49, hence checkCostConvergence = false + CHECK(!gnc.checkCostConvergence(cost, prev_cost)); + } + { + GncParams gncParams; + gncParams.setRelativeCostTol(0.51); + auto gnc = GncOptimizer>(fg, initial, + gncParams); + + double prev_cost = 1.0; + double cost = 0.5; + // relative cost reduction = 0.5 < 0.51, hence checkCostConvergence = true + CHECK(gnc.checkCostConvergence(cost, prev_cost)); + } +} + +/* ************************************************************************* */ +TEST(GncOptimizer, checkWeightsConvergence) { + // has to have Gaussian noise models ! + auto fg = example::createReallyNonlinearFactorGraph(); + + Point2 p0(3, 3); + Values initial; + initial.insert(X(1), p0); + + { + GncParams gncParams; + gncParams.setLossType(GncLossType::GM); + auto gnc = GncOptimizer>(fg, initial, + gncParams); + + Vector weights = Vector::Ones(fg.size()); + CHECK(!gnc.checkWeightsConvergence(weights)); //always false for GM + } + { + GncParams gncParams; + gncParams.setLossType( + GncLossType::TLS); + auto gnc = GncOptimizer>(fg, initial, + gncParams); + + Vector weights = Vector::Ones(fg.size()); + // weights are binary, so checkWeightsConvergence = true + CHECK(gnc.checkWeightsConvergence(weights)); + } + { + GncParams gncParams; + gncParams.setLossType( + GncLossType::TLS); + auto gnc = GncOptimizer>(fg, initial, + gncParams); + + Vector weights = Vector::Ones(fg.size()); + weights[0] = 0.9; // more than weightsTol = 1e-4 from 1, hence checkWeightsConvergence = false + CHECK(!gnc.checkWeightsConvergence(weights)); + } + { + GncParams gncParams; + gncParams.setLossType( + GncLossType::TLS); + gncParams.setWeightsTol(0.1); + auto gnc = GncOptimizer>(fg, initial, + gncParams); + + Vector weights = Vector::Ones(fg.size()); + weights[0] = 0.9; // exactly weightsTol = 0.1 from 1, hence checkWeightsConvergence = true + CHECK(gnc.checkWeightsConvergence(weights)); + } +} + +/* ************************************************************************* */ +TEST(GncOptimizer, checkConvergenceTLS) { + // has to have Gaussian noise models ! + auto fg = example::createReallyNonlinearFactorGraph(); + + Point2 p0(3, 3); + Values initial; + initial.insert(X(1), p0); + + GncParams gncParams; + gncParams.setRelativeCostTol(1e-5); + gncParams.setLossType(GncLossType::TLS); + auto gnc = GncOptimizer>(fg, initial, + gncParams); + + CHECK(gnc.checkCostConvergence(1.0, 1.0)); + CHECK(!gnc.checkCostConvergence(1.0, 2.0)); +} + +/* ************************************************************************* */ +TEST(GncOptimizer, calculateWeightsGM) { + auto fg = example::sharedNonRobustFactorGraphWithOutliers(); + + Point2 p0(0, 0); + Values initial; + initial.insert(X(1), p0); + + // we have 4 factors, 3 with zero errors (inliers), 1 with error 50 = 0.5 * + // 1/sigma^2 || [1;0] - [0;0] ||^2 (outlier) + Vector weights_expected = Vector::Zero(4); + weights_expected[0] = 1.0; // zero error + weights_expected[1] = 1.0; // zero error + weights_expected[2] = 1.0; // zero error + weights_expected[3] = std::pow(1.0 / (50.0 + 1.0), 2); // outlier, error = 50 + + GaussNewtonParams gnParams; + GncParams gncParams(gnParams); + gncParams.setLossType(GncLossType::GM); + auto gnc = GncOptimizer>(fg, initial, gncParams); + double mu = 1.0; + Vector weights_actual = gnc.calculateWeights(initial, mu); + CHECK(assert_equal(weights_expected, weights_actual, tol)); + + mu = 2.0; + double barcSq = 5.0; + weights_expected[3] = std::pow(mu * barcSq / (50.0 + mu * barcSq), 2); // outlier, error = 50 + gncParams.setInlierCostThreshold(barcSq); + auto gnc2 = GncOptimizer>(fg, initial, + gncParams); + weights_actual = gnc2.calculateWeights(initial, mu); + CHECK(assert_equal(weights_expected, weights_actual, tol)); +} + +/* ************************************************************************* */ +TEST(GncOptimizer, calculateWeightsTLS) { + auto fg = example::sharedNonRobustFactorGraphWithOutliers(); + + Point2 p0(0, 0); + Values initial; + initial.insert(X(1), p0); + + // we have 4 factors, 3 with zero errors (inliers), 1 with error + Vector weights_expected = Vector::Zero(4); + weights_expected[0] = 1.0; // zero error + weights_expected[1] = 1.0; // zero error + weights_expected[2] = 1.0; // zero error + weights_expected[3] = 0; // outliers + + GaussNewtonParams gnParams; + GncParams gncParams(gnParams); + gncParams.setLossType(GncLossType::TLS); + auto gnc = GncOptimizer>(fg, initial, gncParams); + double mu = 1.0; + Vector weights_actual = gnc.calculateWeights(initial, mu); + CHECK(assert_equal(weights_expected, weights_actual, tol)); +} + +/* ************************************************************************* */ +TEST(GncOptimizer, calculateWeightsTLS2) { + + // create values + Point2 x_val(0.0, 0.0); + Point2 x_prior(1.0, 0.0); + Values initial; + initial.insert(X(1), x_val); + + // create very simple factor graph with a single factor 0.5 * 1/sigma^2 * || x - [1;0] ||^2 + double sigma = 1; + SharedDiagonal noise = noiseModel::Diagonal::Sigmas(Vector2(sigma, sigma)); + NonlinearFactorGraph nfg; + nfg.add(PriorFactor(X(1), x_prior, noise)); + + // cost of the factor: + DOUBLES_EQUAL(0.5 * 1 / (sigma * sigma), nfg.error(initial), tol); + + // check the TLS weights are correct: CASE 1: residual below barcsq + { + // expected: + Vector weights_expected = Vector::Zero(1); + weights_expected[0] = 1.0; // inlier + // actual: + GaussNewtonParams gnParams; + GncParams gncParams(gnParams); + gncParams.setLossType(GncLossType::TLS); + gncParams.setInlierCostThreshold(0.51); // if inlier threshold is slightly larger than 0.5, then measurement is inlier + auto gnc = GncOptimizer>(nfg, initial, + gncParams); + double mu = 1e6; + Vector weights_actual = gnc.calculateWeights(initial, mu); + CHECK(assert_equal(weights_expected, weights_actual, tol)); + } + // check the TLS weights are correct: CASE 2: residual above barcsq + { + // expected: + Vector weights_expected = Vector::Zero(1); + weights_expected[0] = 0.0; // outlier + // actual: + GaussNewtonParams gnParams; + GncParams gncParams(gnParams); + gncParams.setLossType(GncLossType::TLS); + gncParams.setInlierCostThreshold(0.49); // if inlier threshold is slightly below 0.5, then measurement is outlier + auto gnc = GncOptimizer>(nfg, initial, + gncParams); + double mu = 1e6; // very large mu recovers original TLS cost + Vector weights_actual = gnc.calculateWeights(initial, mu); + CHECK(assert_equal(weights_expected, weights_actual, tol)); + } + // check the TLS weights are correct: CASE 2: residual at barcsq + { + // expected: + Vector weights_expected = Vector::Zero(1); + weights_expected[0] = 0.5; // undecided + // actual: + GaussNewtonParams gnParams; + GncParams gncParams(gnParams); + gncParams.setLossType(GncLossType::TLS); + gncParams.setInlierCostThreshold(0.5); // if inlier threshold is slightly below 0.5, then measurement is outlier + auto gnc = GncOptimizer>(nfg, initial, + gncParams); + double mu = 1e6; // very large mu recovers original TLS cost + Vector weights_actual = gnc.calculateWeights(initial, mu); + CHECK(assert_equal(weights_expected, weights_actual, 1e-5)); + } +} + +/* ************************************************************************* */ +TEST(GncOptimizer, makeWeightedGraph) { + // create original factor + double sigma1 = 0.1; + NonlinearFactorGraph nfg = example::nonlinearFactorGraphWithGivenSigma( + sigma1); + + // create expected + double sigma2 = 10; + NonlinearFactorGraph expected = example::nonlinearFactorGraphWithGivenSigma( + sigma2); + + // create weights + Vector weights = Vector::Ones(1); // original info:1/0.1^2 = 100. New info: 1/10^2 = 0.01. Ratio is 10-4 + weights[0] = 1e-4; + + // create actual + Point2 p0(3, 3); + Values initial; + initial.insert(X(1), p0); + + LevenbergMarquardtParams lmParams; + GncParams gncParams(lmParams); + auto gnc = GncOptimizer>(nfg, initial, + gncParams); + NonlinearFactorGraph actual = gnc.makeWeightedGraph(weights); + + // check it's all good + CHECK(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(GncOptimizer, optimizeSimple) { + auto fg = example::createReallyNonlinearFactorGraph(); + + Point2 p0(3, 3); + Values initial; + initial.insert(X(1), p0); + + LevenbergMarquardtParams lmParams; + GncParams gncParams(lmParams); + auto gnc = GncOptimizer>(fg, initial, + gncParams); + + Values actual = gnc.optimize(); + DOUBLES_EQUAL(0, fg.error(actual), tol); +} + +/* ************************************************************************* */ +TEST(GncOptimizer, optimize) { + auto fg = example::sharedNonRobustFactorGraphWithOutliers(); + + Point2 p0(1, 0); + Values initial; + initial.insert(X(1), p0); + + // try with nonrobust cost function and standard GN + GaussNewtonParams gnParams; + GaussNewtonOptimizer gn(fg, initial, gnParams); + Values gn_results = gn.optimize(); + // converges to incorrect point due to lack of robustness to an outlier, ideal + // solution is Point2(0,0) + CHECK(assert_equal(Point2(0.25, 0.0), gn_results.at(X(1)), 1e-3)); + + // try with robust loss function and standard GN + auto fg_robust = example::sharedRobustFactorGraphWithOutliers(); // same as fg, but with + // factors wrapped in + // Geman McClure losses + GaussNewtonOptimizer gn2(fg_robust, initial, gnParams); + Values gn2_results = gn2.optimize(); + // converges to incorrect point, this time due to the nonconvexity of the loss + CHECK(assert_equal(Point2(0.999706, 0.0), gn2_results.at(X(1)), 1e-3)); + + // .. but graduated nonconvexity ensures both robustness and convergence in + // the face of nonconvexity + GncParams gncParams(gnParams); + // gncParams.setVerbosityGNC(GncParams::Verbosity::SUMMARY); + auto gnc = GncOptimizer>(fg, initial, gncParams); + Values gnc_result = gnc.optimize(); + CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at(X(1)), 1e-3)); +} + +/* ************************************************************************* */ +TEST(GncOptimizer, optimizeWithKnownInliers) { + auto fg = example::sharedNonRobustFactorGraphWithOutliers(); + + Point2 p0(1, 0); + Values initial; + initial.insert(X(1), p0); + + std::vector knownInliers; + knownInliers.push_back(0); + knownInliers.push_back(1); + knownInliers.push_back(2); + + // nonconvexity with known inliers + { + GncParams gncParams; + gncParams.setKnownInliers(knownInliers); + gncParams.setLossType(GncLossType::GM); + //gncParams.setVerbosityGNC(GncParams::Verbosity::SUMMARY); + auto gnc = GncOptimizer>(fg, initial, + gncParams); + + Values gnc_result = gnc.optimize(); + CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at(X(1)), 1e-3)); + + // check weights were actually fixed: + Vector finalWeights = gnc.getWeights(); + DOUBLES_EQUAL(1.0, finalWeights[0], tol); + DOUBLES_EQUAL(1.0, finalWeights[1], tol); + DOUBLES_EQUAL(1.0, finalWeights[2], tol); + } + { + GncParams gncParams; + gncParams.setKnownInliers(knownInliers); + gncParams.setLossType(GncLossType::TLS); + // gncParams.setVerbosityGNC(GncParams::Verbosity::SUMMARY); + auto gnc = GncOptimizer>(fg, initial, + gncParams); + + Values gnc_result = gnc.optimize(); + CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at(X(1)), 1e-3)); + + // check weights were actually fixed: + Vector finalWeights = gnc.getWeights(); + DOUBLES_EQUAL(1.0, finalWeights[0], tol); + DOUBLES_EQUAL(1.0, finalWeights[1], tol); + DOUBLES_EQUAL(1.0, finalWeights[2], tol); + DOUBLES_EQUAL(0.0, finalWeights[3], tol); + } + { + // if we set the threshold large, they are all inliers + GncParams gncParams; + gncParams.setKnownInliers(knownInliers); + gncParams.setLossType(GncLossType::TLS); + //gncParams.setVerbosityGNC(GncParams::Verbosity::VALUES); + gncParams.setInlierCostThreshold(100.0); + auto gnc = GncOptimizer>(fg, initial, + gncParams); + + Values gnc_result = gnc.optimize(); + CHECK(assert_equal(Point2(0.25, 0.0), gnc_result.at(X(1)), 1e-3)); + + // check weights were actually fixed: + Vector finalWeights = gnc.getWeights(); + DOUBLES_EQUAL(1.0, finalWeights[0], tol); + DOUBLES_EQUAL(1.0, finalWeights[1], tol); + DOUBLES_EQUAL(1.0, finalWeights[2], tol); + DOUBLES_EQUAL(1.0, finalWeights[3], tol); + } +} + +/* ************************************************************************* */ +TEST(GncOptimizer, optimizeSmallPoseGraph) { + /// load small pose graph + const string filename = findExampleDataFile("w100.graph"); + NonlinearFactorGraph::shared_ptr graph; + Values::shared_ptr initial; + boost::tie(graph, initial) = load2D(filename); + // Add a Gaussian prior on first poses + Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin + SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas( + Vector3(0.01, 0.01, 0.01)); + graph->addPrior(0, priorMean, priorNoise); + + /// get expected values by optimizing outlier-free graph + Values expected = LevenbergMarquardtOptimizer(*graph, *initial).optimize(); + + // add a few outliers + SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas( + Vector3(0.1, 0.1, 0.01)); + graph->push_back(BetweenFactor(90, 50, Pose2(), betweenNoise)); // some arbitrary and incorrect between factor + + /// get expected values by optimizing outlier-free graph + Values expectedWithOutliers = LevenbergMarquardtOptimizer(*graph, *initial) + .optimize(); + // as expected, the following test fails due to the presence of an outlier! + // CHECK(assert_equal(expected, expectedWithOutliers, 1e-3)); + + // GNC + // Note: in difficult instances, we set the odometry measurements to be + // inliers, but this problem is simple enought to succeed even without that + // assumption std::vector knownInliers; + GncParams gncParams; + auto gnc = GncOptimizer>(*graph, *initial, + gncParams); + Values actual = gnc.optimize(); + + // compare + CHECK(assert_equal(expected, actual, 1e-3)); // yay! we are robust to outliers! +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 662b071df8..84bba850b3 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -233,6 +233,26 @@ TEST( NonlinearFactor, linearize_constraint2 ) CHECK(assert_equal((const GaussianFactor&)expected, *actual)); } +/* ************************************************************************* */ +TEST( NonlinearFactor, cloneWithNewNoiseModel ) +{ + // create original factor + double sigma1 = 0.1; + NonlinearFactorGraph nfg = example::nonlinearFactorGraphWithGivenSigma(sigma1); + + // create expected + double sigma2 = 10; + NonlinearFactorGraph expected = example::nonlinearFactorGraphWithGivenSigma(sigma2); + + // create actual + NonlinearFactorGraph actual; + SharedNoiseModel noise2 = noiseModel::Isotropic::Sigma(2,sigma2); + actual.push_back( boost::dynamic_pointer_cast(nfg[0])->cloneWithNewNoiseModel(noise2) ); + + // check it's all good + CHECK(assert_equal(expected, actual)); +} + /* ************************************************************************* */ class TestFactor4 : public NoiseModelFactor4 { public: diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 6415174d5b..295721cc42 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -48,6 +48,19 @@ const double tol = 1e-5; using symbol_shorthand::X; using symbol_shorthand::L; +/* ************************************************************************* */ +TEST( NonlinearOptimizer, paramsEquals ) +{ + // default constructors lead to two identical params + GaussNewtonParams gnParams1; + GaussNewtonParams gnParams2; + CHECK(gnParams1.equals(gnParams2)); + + // but the params become different if we change something in gnParams2 + gnParams2.setVerbosity("DELTA"); + CHECK(!gnParams1.equals(gnParams2)); +} + /* ************************************************************************* */ TEST( NonlinearOptimizer, iterateLM ) {