From 41684ee6e059f66539e95ff811ac32b9a465cc06 Mon Sep 17 00:00:00 2001 From: = Date: Fri, 17 Jun 2016 12:29:03 -0400 Subject: [PATCH] Deleted unrealted SQP files. --- gtsam_unstable/nonlinear/ConstrainedFactor.h | 54 -- .../nonlinear/LinearConstraintSQP.cpp | 133 ----- .../nonlinear/LinearConstraintSQP.h | 135 ----- .../nonlinear/LinearEqualityFactor.h | 192 ------ .../nonlinear/LinearEqualityFactorGraph.cpp | 52 -- .../nonlinear/LinearEqualityFactorGraph.h | 45 -- .../nonlinear/LinearInequalityFactor.h | 138 ----- .../nonlinear/LinearInequalityFactorGraph.cpp | 69 --- .../nonlinear/LinearInequalityFactorGraph.h | 46 -- .../nonlinear/NonlinearConstraint.h | 552 ------------------ gtsam_unstable/nonlinear/tests/CMakeLists.txt | 2 +- .../nonlinear/tests/testLCNLPSolver2.cpp | 335 ----------- .../tests/testLinearConstraintSQP.cpp | 223 ------- .../tests/testLinearInequalityFactorGraph.cpp | 42 -- .../tests/testLinearInequalityManifolds.cpp | 68 --- .../tests/testNonlinearConstraints.cpp | 176 ------ 16 files changed, 1 insertion(+), 2261 deletions(-) delete mode 100644 gtsam_unstable/nonlinear/ConstrainedFactor.h delete mode 100644 gtsam_unstable/nonlinear/LinearConstraintSQP.cpp delete mode 100644 gtsam_unstable/nonlinear/LinearConstraintSQP.h delete mode 100644 gtsam_unstable/nonlinear/LinearEqualityFactor.h delete mode 100644 gtsam_unstable/nonlinear/LinearEqualityFactorGraph.cpp delete mode 100644 gtsam_unstable/nonlinear/LinearEqualityFactorGraph.h delete mode 100644 gtsam_unstable/nonlinear/LinearInequalityFactor.h delete mode 100644 gtsam_unstable/nonlinear/LinearInequalityFactorGraph.cpp delete mode 100644 gtsam_unstable/nonlinear/LinearInequalityFactorGraph.h delete mode 100644 gtsam_unstable/nonlinear/NonlinearConstraint.h delete mode 100644 gtsam_unstable/nonlinear/tests/testLCNLPSolver2.cpp delete mode 100644 gtsam_unstable/nonlinear/tests/testLinearConstraintSQP.cpp delete mode 100644 gtsam_unstable/nonlinear/tests/testLinearInequalityFactorGraph.cpp delete mode 100644 gtsam_unstable/nonlinear/tests/testLinearInequalityManifolds.cpp delete mode 100644 gtsam_unstable/nonlinear/tests/testNonlinearConstraints.cpp diff --git a/gtsam_unstable/nonlinear/ConstrainedFactor.h b/gtsam_unstable/nonlinear/ConstrainedFactor.h deleted file mode 100644 index ccb42dbed..000000000 --- a/gtsam_unstable/nonlinear/ConstrainedFactor.h +++ /dev/null @@ -1,54 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 ConstrainedFactor.h - * @brief - * @author Duy-Nguyen Ta - * @date Sep 30, 2013 - */ - -#pragma once - -#include -#include - -namespace gtsam { - -/** - * A base class for all ConstrainedFactor factors, - * containing additional information for the constraint, - * e.g., a unique key for the dual variable associated with it, - * and special treatments for nonlinear constraint linearization in SQP. - * - * Derived classes of ConstrainedFactor should also inherit from - * NoiseModelFactorX to reuse the normal linearization procedure in NonlinearFactor - */ -class ConstrainedFactor { - -protected: - Key dualKey_; //!< Unique key for the dual variable associated with this constraint - -public: - typedef boost::shared_ptr shared_ptr; - -public: - /// Default constructor with default key value. - ConstrainedFactor() : dualKey_(0) {} - /// Construct with dual key - ConstrainedFactor(Key dualKey) : dualKey_(dualKey) {} - - /// Return the dual key - Key dualKey() const { return dualKey_; } - -}; - - -} /* namespace gtsam */ diff --git a/gtsam_unstable/nonlinear/LinearConstraintSQP.cpp b/gtsam_unstable/nonlinear/LinearConstraintSQP.cpp deleted file mode 100644 index f8734d81a..000000000 --- a/gtsam_unstable/nonlinear/LinearConstraintSQP.cpp +++ /dev/null @@ -1,133 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 LinearConstraintSQP.cpp - * @author Duy-Nguyen Ta - * @author Krunal Chande - * @author Luca Carlone - * @date Dec 15, 2014 - */ - -#include -#include -#include -#include -#include - -namespace gtsam { - -/* ************************************************************************* */ -bool LinearConstraintSQP::isStationary(const VectorValues& delta) const { - return delta.vector().lpNorm() < params_.errorTol; -} - -/* ************************************************************************* */ -bool LinearConstraintSQP::isPrimalFeasible(const LinearConstraintNLPState& state) const { - return lcnlp_.linearEqualities.checkFeasibility(state.values, params_.errorTol); -} - -/* ************************************************************************* */ -bool LinearConstraintSQP::isDualFeasible(const VectorValues& duals) const { - for(const NonlinearFactor::shared_ptr& factor: lcnlp_.linearInequalities) { - ConstrainedFactor::shared_ptr inequality - = boost::dynamic_pointer_cast(factor); - - Key dualKey = inequality->dualKey(); - if (!duals.exists(dualKey)) continue; // should be inactive constraint! - double dual = duals.at(dualKey)[0];// because we only support single-valued inequalities - if (dual > 0.0) // See the explanation in QPSolver::identifyLeavingConstraint, we want dual < 0 ? - return false; - } - return true; -} - -/* ************************************************************************* */ -bool LinearConstraintSQP::isComplementary(const LinearConstraintNLPState& state) const { - return lcnlp_.linearInequalities.checkFeasibilityAndComplimentary( - state.values, state.duals, params_.errorTol); -} - -/* ************************************************************************* */ -bool LinearConstraintSQP::checkConvergence(const LinearConstraintNLPState& state, - const VectorValues& delta) const { - return isStationary(delta) && isPrimalFeasible(state) - && isDualFeasible(state.duals) && isComplementary(state); -} - -/* ************************************************************************* */ -VectorValues LinearConstraintSQP::initializeDuals() const { - VectorValues duals; - for(const NonlinearFactor::shared_ptr& factor: lcnlp_.linearEqualities){ - ConstrainedFactor::shared_ptr constraint - = boost::dynamic_pointer_cast(factor); - duals.insert(constraint->dualKey(), Vector::Zero(factor->dim())); - } - - return duals; -} - -/* ************************************************************************* */ -LinearConstraintNLPState LinearConstraintSQP::iterate( - const LinearConstraintNLPState& state) const { - - // construct the qp subproblem - QP qp; - qp.cost = *lcnlp_.cost.linearize(state.values); - qp.equalities.add(*lcnlp_.linearEqualities.linearize(state.values)); - qp.inequalities.add(*lcnlp_.linearInequalities.linearize(state.values)); - - if(params_.verbosity >= NonlinearOptimizerParams::LINEAR) - qp.print("QP subproblem:"); - - // solve the QP subproblem - VectorValues delta, duals; - QPSolver qpSolver(qp); - VectorValues zeroInitialValues; - for(const Values::ConstKeyValuePair& key_value: state.values) - zeroInitialValues.insert(key_value.key, Vector::Zero(key_value.value.dim())); - - boost::tie(delta, duals) = qpSolver.optimize(zeroInitialValues, state.duals, - params_.warmStart); - - if(params_.verbosity >= NonlinearOptimizerParams::DELTA) - delta.print("Delta"); - - // update new state - LinearConstraintNLPState newState; - newState.values = state.values.retract(delta); - newState.duals = duals; - newState.converged = checkConvergence(newState, delta); - newState.iterations = state.iterations + 1; - - if(params_.verbosity >= NonlinearOptimizerParams::VALUES) - newState.print("Values"); - - return newState; -} - -/* ************************************************************************* */ -std::pair LinearConstraintSQP::optimize( - const Values& initialValues) const { - LinearConstraintNLPState state(initialValues); - state.duals = initializeDuals(); - while (!state.converged && state.iterations < params_.maxIterations) { - if(params_.verbosity >= NonlinearOptimizerParams::ERROR) - std::cout << "Iteration # " << state.iterations << std::endl; - state = iterate(state); - } - if(params_.verbosity >= NonlinearOptimizerParams::TERMINATION) - std::cout << "Number of iterations: " << state.iterations << std::endl; - return std::make_pair(state.values, state.duals); -} - -} // namespace gtsam - diff --git a/gtsam_unstable/nonlinear/LinearConstraintSQP.h b/gtsam_unstable/nonlinear/LinearConstraintSQP.h deleted file mode 100644 index f0665807e..000000000 --- a/gtsam_unstable/nonlinear/LinearConstraintSQP.h +++ /dev/null @@ -1,135 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 LinearConstraintSQP.h - * @author Duy-Nguyen Ta - * @author Krunal Chande - * @author Luca Carlone - * @date Dec 15, 2014 - */ - -#pragma once -#include -#include -#include -#include - -namespace gtsam { - -/** - * Nonlinear Programming problem with - * only linear constraints, encoded in factor graphs - */ -struct LinearConstraintNLP { - NonlinearFactorGraph cost; - LinearEqualityFactorGraph linearEqualities; - LinearInequalityFactorGraph linearInequalities; -}; - -/** - * State of LinearConstraintSQP before/after each iteration - */ -struct LinearConstraintNLPState { - Values values; //!< current solution - VectorValues duals; //!< current guess of the dual variables - bool converged; //!< convergence flag - size_t iterations; //!< number of iterations - - /// Default constructor - LinearConstraintNLPState() : values(), duals(), converged(false), iterations(0) {} - - /// Constructor with an initialValues - LinearConstraintNLPState(const Values& initialValues) : - values(initialValues), duals(VectorValues()), converged(false), iterations(0) { - } - - /// print - void print(const std::string& s = "") const { - std::cout << s << std::endl; - values.print("Values: "); - duals.print("Duals: "); - if (converged) std::cout << "Converged!" << std::endl; - else std::cout << "Not converged" << std::endl; - std::cout << "Iterations: " << iterations << std::endl; - } -}; - -/** Parameters for Gauss-Newton optimization, inherits from - * NonlinearOptimizationParams. - */ -class GTSAM_EXPORT LinearConstraintSQPParams : public NonlinearOptimizerParams { -public: - bool warmStart; - - LinearConstraintSQPParams() : NonlinearOptimizerParams(), warmStart(false) {} - - void setWarmStart(bool _warmStart) { - _warmStart = warmStart; - } -}; - -/** - * Simple SQP optimizer to solve nonlinear constrained problems - * ONLY linear constraints are supported. - */ -class LinearConstraintSQP { - LinearConstraintNLP lcnlp_; - LinearConstraintSQPParams params_; - -public: - LinearConstraintSQP(const LinearConstraintNLP& lcnlp, - const LinearConstraintSQPParams& params = LinearConstraintSQPParams()) : - lcnlp_(lcnlp), params_(params) { - } - - /// Check if \nabla f(x) - \lambda * \nabla c(x) == 0 - bool isStationary(const VectorValues& delta) const; - - /// Check if c_E(x) == 0 - bool isPrimalFeasible(const LinearConstraintNLPState& state) const; - - /** - * Dual variables of inequality constraints need to be >=0 - * For active inequalities, the dual needs to be > 0 - * For inactive inequalities, they need to be == 0. However, we don't compute - * dual variables for inactive constraints in the qp subproblem, so we don't care. - */ - bool isDualFeasible(const VectorValues& duals) const; - - /** - * Check complimentary slackness condition: - * For all inequality constraints, - * dual * constraintError(primals) == 0. - * If the constraint is active, we need to check constraintError(primals) == 0, and ignore the dual - * If it is inactive, the dual should be 0, regardless of the error. However, we don't compute - * dual variables for inactive constraints in the QP subproblem, so we don't care. - */ - bool isComplementary(const LinearConstraintNLPState& state) const; - - /// Check convergence - bool checkConvergence(const LinearConstraintNLPState& state, const VectorValues& delta) const; - - /** - * Single iteration of SQP - */ - LinearConstraintNLPState iterate(const LinearConstraintNLPState& state) const; - - /// Intialize all dual variables to zeros - VectorValues initializeDuals() const; - - /** - * Main optimization function. new - */ - std::pair optimize(const Values& initialValues) const; - -}; -} diff --git a/gtsam_unstable/nonlinear/LinearEqualityFactor.h b/gtsam_unstable/nonlinear/LinearEqualityFactor.h deleted file mode 100644 index 716bc0db2..000000000 --- a/gtsam_unstable/nonlinear/LinearEqualityFactor.h +++ /dev/null @@ -1,192 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 LinearEqualityFactor.h - * @brief Linear equality factors at the nonlinear level - * @author Duy-Nguyen Ta - * @date Sep 30, 2013 - */ - -#pragma once - -#include -#include - -namespace gtsam { - -/* ************************************************************************* */ -/** A convenient base class for creating a linear equality constraint with 1 - * variables. To derive from this class, implement evaluateError(). - * Warning: It is the user's responsibility to make sure the Hessian is approximately zero - * - * TODO: Should we check Hessian = 0 automatically to make sure it's linear, like SNOPT? - * TODO: Sometimes the true Hessian is not 0, but an approximate one with a different retract is zero - * and that could also work [Absil07fcm] - */ -template -class LinearEqualityFactor1: public NoiseModelFactor1, - public ConstrainedFactor { - -public: - // typedefs for value types pulled from keys - typedef VALUE X; - -protected: - typedef NoiseModelFactor1 Base; - typedef LinearEqualityFactor1 This; - -public: - /** - * Default Constructor for I/O - */ - LinearEqualityFactor1() { - } - - /** - * Constructor - * @param j key of the variable - * @param constraintDim number of dimensions of the constraint error function - */ - LinearEqualityFactor1(Key key, Key dualKey, size_t constraintDim = 1) : - Base(noiseModel::Constrained::All(constraintDim), key), ConstrainedFactor( - dualKey) { - } - - virtual ~LinearEqualityFactor1() { - } - - /** - * 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 X&, boost::optional H1 = boost::none) const = 0; - -}; -// \class LinearEqualityFactor1 - -/* ************************************************************************* */ -/** A convenient base class for creating a linear equality constraint with 2 - * variables. To derive from this class, implement evaluateError(). - * Warning: It is the user's responsibility to make sure the Hessian is approximately zero - * - * TODO: Should we check Hessian = 0 automatically to make sure it's linear, like SNOPT? - * TODO: Sometimes the true Hessian is not 0, but an approximate one with a different retract is zero - * and that could also work [Absil07fcm] - */ -template -class LinearEqualityFactor2: public NoiseModelFactor2, - public ConstrainedFactor { - -public: - // typedefs for value types pulled from keys - typedef VALUE1 X1; - typedef VALUE2 X2; - -protected: - typedef NoiseModelFactor2 Base; - typedef LinearEqualityFactor2 This; - -public: - /** - * Default Constructor for I/O - */ - LinearEqualityFactor2() { - } - - /** - * Constructor - * @param j1 key of the first variable - * @param j2 key of the second variable - * @param constraintDim number of dimensions of the constraint error function - */ - LinearEqualityFactor2(Key j1, Key j2, Key dualKey, size_t constraintDim = 1) : - Base(noiseModel::Constrained::All(constraintDim), j1, j2), ConstrainedFactor( - dualKey) { - } - - virtual ~LinearEqualityFactor2() { - } - - /** - * 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; - -}; -// \class LinearEqualityFactor2 - -/* ************************************************************************* */ -/** A convenient base class for creating a linear equality constraint with 3 - * variables. To derive from this class, implement evaluateError(). - * Warning: It is the user's responsibility to make sure the Hessian is approximately zero - * - * TODO: Should we check Hessian = 0 automatically to make sure it's linear, like SNOPT? - * TODO: Sometimes the true Hessian is not 0, but an approximate one with a different retract is zero - * and that could also work [Absil07fcm] - */ -template -class LinearEqualityFactor3: public NoiseModelFactor3, - public ConstrainedFactor { - -public: - // typedefs for value types pulled from keys - typedef VALUE1 X1; - typedef VALUE2 X2; - typedef VALUE3 X3; - -protected: - typedef NoiseModelFactor3 Base; - typedef LinearEqualityFactor3 This; - -public: - - /** - * Default Constructor for I/O - */ - LinearEqualityFactor3() { - } - - /** - * Constructor - * @param j1 key of the first variable - * @param j2 key of the second variable - * @param constraintDim number of dimensions of the constraint error function - */ - LinearEqualityFactor3(Key j1, Key j2, Key j3, Key dualKey, - size_t constraintDim = 1) : - Base(noiseModel::Constrained::All(constraintDim), j1, j2, j3), ConstrainedFactor( - dualKey) { - } - - virtual ~LinearEqualityFactor3() { - } - - /** - * 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&, const X3&, boost::optional H1 = - boost::none, boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const = 0; - -}; -// \class LinearEqualityFactor3 - -} /* namespace gtsam */ diff --git a/gtsam_unstable/nonlinear/LinearEqualityFactorGraph.cpp b/gtsam_unstable/nonlinear/LinearEqualityFactorGraph.cpp deleted file mode 100644 index 1dc033ed1..000000000 --- a/gtsam_unstable/nonlinear/LinearEqualityFactorGraph.cpp +++ /dev/null @@ -1,52 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 LinearEqualityFactorGraph.cpp - * @author Duy-Nguyen Ta - * @author Krunal Chande - * @author Luca Carlone - * @date Dec 15, 2014 - */ - -#include -#include - -namespace gtsam { - -/* ************************************************************************* */ -EqualityFactorGraph::shared_ptr LinearEqualityFactorGraph::linearize( - const Values& linearizationPoint) const { - EqualityFactorGraph::shared_ptr linearGraph( - new EqualityFactorGraph()); - for(const NonlinearFactor::shared_ptr& factor: *this){ - JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast( - factor->linearize(linearizationPoint)); - ConstrainedFactor::shared_ptr constraint = boost::dynamic_pointer_cast(factor); - linearGraph->add(LinearEquality(*jacobian, constraint->dualKey())); - } - return linearGraph; -} - -/* ************************************************************************* */ -bool LinearEqualityFactorGraph::checkFeasibility(const Values& values, double tol) const { - for(const NonlinearFactor::shared_ptr& factor: *this){ - NoiseModelFactor::shared_ptr noiseModelFactor - = boost::dynamic_pointer_cast(factor); - Vector error = noiseModelFactor->unwhitenedError(values); - - if (error.lpNorm() > tol) - return false; - } - return true; -} - -} diff --git a/gtsam_unstable/nonlinear/LinearEqualityFactorGraph.h b/gtsam_unstable/nonlinear/LinearEqualityFactorGraph.h deleted file mode 100644 index dd481fdba..000000000 --- a/gtsam_unstable/nonlinear/LinearEqualityFactorGraph.h +++ /dev/null @@ -1,45 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 LinearEqualityFactorGraph.h - * @author Duy-Nguyen Ta - * @author Krunal Chande - * @author Luca Carlone - * @date Dec 15, 2014 - */ - -#pragma once -#include -#include -#include -#include - -namespace gtsam { - -/** - * FactorGraph container for linear equality factors c(x)==0 at the nonlinear level - */ -class LinearEqualityFactorGraph: public FactorGraph { -public: - /// Default constructor - LinearEqualityFactorGraph() { - } - - /// Linearize to a EqualityFactorGraph - EqualityFactorGraph::shared_ptr linearize(const Values& linearizationPoint) const; - - /// Return true if the max absolute error all factors is less than a tolerance - bool checkFeasibility(const Values& values, double tol) const; - -}; - -} diff --git a/gtsam_unstable/nonlinear/LinearInequalityFactor.h b/gtsam_unstable/nonlinear/LinearInequalityFactor.h deleted file mode 100644 index 9923d58a4..000000000 --- a/gtsam_unstable/nonlinear/LinearInequalityFactor.h +++ /dev/null @@ -1,138 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 LinearInequalityFactor.h - * @brief - * @author Duy-Nguyen Ta - * @date Sep 30, 2013 - */ - -#pragma once - -#include - -namespace gtsam { - -/* ************************************************************************* */ -/** - * A convenient base class for creating a linear inequality constraint, e.g., Ax <= 0, - * at the nonlinear level with 1 variable. - * To derive from this class, implement computeError(). - */ -template -class LinearInequalityFactor1: public LinearEqualityFactor1 { - -public: - - // typedefs for value types pulled from keys - typedef VALUE X; - -protected: - typedef LinearEqualityFactor1 Base; - typedef LinearInequalityFactor1 This; - -public: - - /** - * Default Constructor for I/O - */ - LinearInequalityFactor1() { - } - - /** - * Constructor - * @param j key of the variable - * @param constraintDim number of dimensions of the constraint error function - */ - LinearInequalityFactor1(Key key, Key dualKey) : - Base(key, dualKey, 1) { - } - - virtual ~LinearInequalityFactor1() { - } - - /** - * 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 double - computeError(const X&, boost::optional H1 = boost::none) const = 0; - - /** predefine evaluateError to return a 1-dimension vector */ - virtual Vector - evaluateError(const X& x, boost::optional H1 = boost::none) const { - return (Vector(1) << computeError(x, H1)).finished(); - } - -}; -// \class LinearEqualityFactor1 - -/* ************************************************************************* */ -/** - * A convenient base class for creating a linear inequality constraint, e.g., Ax <= 0, - * at the nonlinear level with 1 variable. - * To derive from this class, implement computeError(). - */ -template -class LinearInequalityFactor2: public LinearEqualityFactor2 { - -public: - - // typedefs for value types pulled from keys - typedef VALUE1 X1; - typedef VALUE2 X2; - -protected: - typedef LinearEqualityFactor2 Base; - typedef LinearInequalityFactor2 This; - -public: - - /** - * Default Constructor for I/O - */ - LinearInequalityFactor2() { - } - - /** - * Constructor - * @param j1 key of the first variable - * @param j2 key of the second variable - * @param constraintDim number of dimensions of the constraint error function - */ - LinearInequalityFactor2(Key j1, Key j2, Key dualKey) : - Base(j1, j2, 1) { - } - - virtual ~LinearInequalityFactor2() { - } - - /** - * 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 double - computeError(const X1&, const X2&, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const = 0; - - /** predefine evaluateError to return a 1-dimension vector */ - virtual Vector - evaluateError(const X1& x1, const X2& x2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { - return (Vector(1) << computeError(x1, x2, H1, H2)).finished(); - } -}; -// \class LinearEqualityFactor2 - - -} /* namespace gtsam */ diff --git a/gtsam_unstable/nonlinear/LinearInequalityFactorGraph.cpp b/gtsam_unstable/nonlinear/LinearInequalityFactorGraph.cpp deleted file mode 100644 index ca10827c3..000000000 --- a/gtsam_unstable/nonlinear/LinearInequalityFactorGraph.cpp +++ /dev/null @@ -1,69 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 LinearInequalityFactorGraph.cpp - * @author Duy-Nguyen Ta - * @author Krunal Chande - * @author Luca Carlone - * @date Dec 15, 2014 - */ - -#include -#include - -namespace gtsam { - -/* ************************************************************************* */ -InequalityFactorGraph::shared_ptr LinearInequalityFactorGraph::linearize( - const Values& linearizationPoint) const { - InequalityFactorGraph::shared_ptr linearGraph(new InequalityFactorGraph()); - for(const NonlinearFactor::shared_ptr& factor: *this) { - JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast( - factor->linearize(linearizationPoint)); - ConstrainedFactor::shared_ptr constraint - = boost::dynamic_pointer_cast(factor); - linearGraph->add(LinearInequality(*jacobian, constraint->dualKey())); - } - return linearGraph; -} - -/* ************************************************************************* */ -bool LinearInequalityFactorGraph::checkFeasibilityAndComplimentary( - const Values& values, const VectorValues& dualValues, double tol) const { - - for(const NonlinearFactor::shared_ptr& factor: *this) { - NoiseModelFactor::shared_ptr noiseModelFactor - = boost::dynamic_pointer_cast(factor); - Vector error = noiseModelFactor->unwhitenedError(values); - - // Primal feasibility condition: all constraints need to be <= 0.0 - if (error[0] > tol) - return false; - - // Complimentary condition: errors of active constraints need to be 0.0 - ConstrainedFactor::shared_ptr constraint - = boost::dynamic_pointer_cast(factor); - Key dualKey = constraint->dualKey(); - - // if dualKey doesn't exist in dualValues, it must be an inactive constraint! - if (!dualValues.exists(dualKey)) - continue; - - // for active constraint, the error should be 0.0 - if (fabs(error[0]) > tol) - return false; - } - - return true; -} - -} diff --git a/gtsam_unstable/nonlinear/LinearInequalityFactorGraph.h b/gtsam_unstable/nonlinear/LinearInequalityFactorGraph.h deleted file mode 100644 index 5dce1ddc5..000000000 --- a/gtsam_unstable/nonlinear/LinearInequalityFactorGraph.h +++ /dev/null @@ -1,46 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 LinearInequalityFactorGraph.h - * @author Duy-Nguyen Ta - * @author Krunal Chande - * @author Luca Carlone - * @date Dec 15, 2014 - */ - -#pragma once -#include -#include -#include -#include - -namespace gtsam { - -/** - * FactorGraph container for linear inequality factors c(x)<=0 at the nonlinear level - */ -class LinearInequalityFactorGraph: public FactorGraph { - -public: - /// Default constructor - LinearInequalityFactorGraph() {} - - /// Linearize to a InequalityFactorGraph - InequalityFactorGraph::shared_ptr linearize(const Values& linearizationPoint) const; - - /// Return true if the all errors are <= 0.0 - bool checkFeasibilityAndComplimentary(const Values& values, - const VectorValues& dualValues, double tol) const; - -}; - -} // namespace gtsam diff --git a/gtsam_unstable/nonlinear/NonlinearConstraint.h b/gtsam_unstable/nonlinear/NonlinearConstraint.h deleted file mode 100644 index 58af9d61e..000000000 --- a/gtsam_unstable/nonlinear/NonlinearConstraint.h +++ /dev/null @@ -1,552 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 NonlinearConstraint.h - * @brief - * @author Duy-Nguyen Ta - * @date Sep 30, 2013 - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -namespace gtsam { - -/* ************************************************************************* */ -/** A convenient base class for creating a nonlinear equality constraint with 1 - * variables. To derive from this class, implement evaluateError(). */ -template -class NonlinearConstraint1: public NoiseModelFactor1, public ConstrainedFactor { - -public: - - // typedefs for value types pulled from keys - typedef VALUE X; - -protected: - - typedef NoiseModelFactor1 Base; - typedef NonlinearConstraint1 This; - -private: - static const int X1Dim = traits::dimension; - -public: - - /** - * Default Constructor for I/O - */ - NonlinearConstraint1() { - } - - /** - * Constructor - * @param j key of the variable - * @param constraintDim number of dimensions of the constraint error function - */ - NonlinearConstraint1(Key key, Key dualKey, size_t constraintDim = 1) : - Base(noiseModel::Constrained::All(constraintDim), key), ConstrainedFactor(dualKey) { - } - - virtual ~NonlinearConstraint1() { - } - - /** - * 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 X&, boost::optional H1 = boost::none) const = 0; - - /** produce a Gaussian factor graph containing all Hessian Factors, scaled by lambda, - * corresponding to this constraint */ - virtual GaussianFactor::shared_ptr multipliedHessian(const Values& x, - const VectorValues& duals) const { - if (!this->active(x)) { - return GaussianFactor::shared_ptr(); - } - const X& x1 = x.at < X > (Base::key()); - const Vector& lambda = duals.at(this->dualKey()); - - std::vector G11; - evaluateHessians(x1, G11); - - if (lambda.size() != G11.size()) { - throw std::runtime_error( - "Error in evaluateHessians: the number of returned Gij matrices must be the same as the constraint dimension!"); - } - - // Combine the Lagrange-multiplier part into this constraint factor - Matrix lG11sum = Matrix::Zero(G11[0].rows(), G11[0].cols()); - for (size_t i = 0; i < lambda.rows(); ++i) { - lG11sum += -lambda[i] * G11[i]; - } - - HessianFactor::shared_ptr hf(new HessianFactor(Base::key(), lG11sum, - Vector::Zero(X1Dim), 100.0)); - return hf; - } - - /** evaluate Hessians for lambda factors */ - virtual void evaluateHessians(const X& x1, std::vector& G11) const { - - static const bool debug = false; - typedef Eigen::Matrix actual_size; - boost::function vecH1( - boost::bind(&This::vectorizeH1t, this, _1)); - - Matrix G11all = numericalDerivative11(vecH1, x1, 1e-5); - - if (debug) { - gtsam::print(G11all, "G11all: "); - std::cout << "x1dim: " << X1Dim << std::endl; - } - - for (size_t i = 0; i < Base::get_noiseModel()->dim(); ++i) { - G11.push_back(G11all.block(i * X1Dim, 0, X1Dim, X1Dim)); - if (debug) - gtsam::print(G11[i], "G11: "); - } - } - -private: - /** Vectorize the transpose of Jacobian H1 to compute the Hessian numerically */ - Vector vectorizeH1t(const X& x1) const { - Matrix H1; - evaluateError(x1, H1); - Matrix H1t = H1.transpose(); - H1t.resize(H1t.size(), 1); - return Vector(H1t); - } - - /** 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 NonlinearConstraint1 - -/* ************************************************************************* */ -/** A convenient base class for creating your own ConstrainedFactor with 2 - * variables. To derive from this class, implement evaluateError(). */ -template -class NonlinearConstraint2: public NoiseModelFactor2, public ConstrainedFactor { - -public: - - // typedefs for value types pulled from keys - typedef VALUE1 X1; - typedef VALUE2 X2; - -protected: - - typedef NoiseModelFactor2 Base; - typedef NonlinearConstraint2 This; - -private: - static const int X1Dim = traits::dimension; - static const int X2Dim = traits::dimension; - -public: - - /** - * Default Constructor for I/O - */ - NonlinearConstraint2() { - } - - /** - * Constructor - * @param j1 key of the first variable - * @param j2 key of the second variable - * @param constraintDim number of dimensions of the constraint error function - */ - NonlinearConstraint2(Key j1, Key j2, Key dualKey, size_t constraintDim = 1) : - Base(noiseModel::Constrained::All(constraintDim), j1, j2), ConstrainedFactor(dualKey) { - } - - virtual ~NonlinearConstraint2() { - } - - /** - * 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; - - /** produce a Gaussian factor graph containing all Hessian Factors, scaled by lambda, - * corresponding to this constraint */ - virtual GaussianFactor::shared_ptr multipliedHessian(const Values& x, - const VectorValues& duals) const { - if (!this->active(x)) { - return GaussianFactor::shared_ptr(); - } - const X1& x1 = x.at < X1 > (Base::keys_[0]); - const X2& x2 = x.at < X2 > (Base::keys_[1]); - const Vector& lambda = duals.at(this->dualKey()); - - std::vector G11, G12, G22; - evaluateHessians(x1, x2, G11, G12, G22); - - if (lambda.size() != G11.size() || lambda.size() != G12.size() - || lambda.size() != G22.size()) { - throw std::runtime_error( - "Error in evaluateHessians: the number of returned Gij matrices must be the same as the constraint dimension!"); - } - - // Combine the Lagrange-multiplier part into this constraint factor - Matrix lG11sum = Matrix::Zero(G11[0].rows(), G11[0].cols()), lG12sum = - Matrix::Zero(G12[0].rows(), G12[0].cols()), lG22sum = Matrix::Zero( - G22[0].rows(), - G22[0].cols()); - for (size_t i = 0; i < lambda.rows(); ++i) { - lG11sum += -lambda[i] * G11[i]; - lG12sum += -lambda[i] * G12[i]; - lG22sum += -lambda[i] * G22[i]; - } - - return boost::make_shared(Base::keys_[0], Base::keys_[1], - lG11sum, lG12sum, Vector::Zero( - X1Dim), lG22sum, Vector::Zero(X2Dim), 0.0); - } - - /** evaluate Hessians for lambda factors */ - virtual void evaluateHessians(const X1& x1, const X2& x2, - std::vector& G11, std::vector& G12, - std::vector& G22) const { - - static const bool debug = false; - - boost::function vecH1( - boost::bind(&This::vectorizeH1t, this, _1, _2)), vecH2( - boost::bind(&This::vectorizeH2t, this, _1, _2)); - - Matrix G11all = numericalDerivative21(vecH1, x1, x2, 1e-5); - Matrix G12all = numericalDerivative22(vecH1, x1, x2, 1e-5); - Matrix G22all = numericalDerivative22(vecH2, x1, x2, 1e-5); - - if (debug) { - gtsam::print(G11all, "G11all: "); - gtsam::print(G12all, "G12all: "); - gtsam::print(G22all, "G22all: "); - std::cout << "x1dim: " << traits::dimension << std::endl; - std::cout << "x2dim: " << traits::dimension << std::endl; - } - - for (size_t i = 0; i < Base::get_noiseModel()->dim(); ++i) { - G11.push_back(G11all.block(i * X1Dim, 0, X1Dim, X1Dim)); - if (debug) - gtsam::print(G11[i], "G11: "); - - G12.push_back(G12all.block(i * X1Dim, 0, X1Dim, X2Dim)); - if (debug) - gtsam::print(G12[i], "G12: "); - - G22.push_back(G22all.block(i * X2Dim, 0, X2Dim, X2Dim)); - if (debug) - gtsam::print(G22[i], "G22: "); - } - } - -private: - /** Vectorize the transpose of Jacobian H1 to compute the Hessian numerically */ - Vector vectorizeH1t(const X1& x1, const X2& x2) const { - Matrix H1; - Vector err = evaluateError(x1, x2, H1); - Matrix H1t = H1.transpose(); - H1t.resize(H1t.size(), 1); - return Vector(H1t); - } - - /** Vectorize the transpose of Jacobian H2 to compute the Hessian numerically */ - Vector vectorizeH2t(const X1& x1, const X2& x2) const { - Matrix H2; - Vector err = evaluateError(x1, x2, boost::none, H2); - Matrix H2t = H2.transpose(); - H2t.resize(H2t.size(), 1); - return Vector(H2t); - } - - /** 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 NonlinearConstraint2 - -/* ************************************************************************* */ -/** A convenient base class for creating your own ConstrainedFactor with 3 - * variables. To derive from this class, implement evaluateError(). */ -template -class NonlinearConstraint3: public NoiseModelFactor3, public ConstrainedFactor { - -public: - - // typedefs for value types pulled from keys - typedef VALUE1 X1; - typedef VALUE2 X2; - typedef VALUE3 X3; - -protected: - - typedef NoiseModelFactor3 Base; - typedef NonlinearConstraint3 This; - -private: - static const int X1Dim = traits::dimension; - static const int X2Dim = traits::dimension; - static const int X3Dim = traits::dimension; - -public: - - /** - * Default Constructor for I/O - */ - NonlinearConstraint3(){ - } - - /** - * Constructor - * @param j1 key of the first variable - * @param j2 key of the second variable - * @param constraintDim number of dimensions of the constraint error function - */ - NonlinearConstraint3(Key j1, Key j2, Key j3, Key dualKey, size_t constraintDim = 1) : - Base(noiseModel::Constrained::All(constraintDim), j1, j2, j3), ConstrainedFactor(dualKey) { - } - - virtual ~NonlinearConstraint3() { - } - - /** - * 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&, const X3&, boost::optional H1 = - boost::none, boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const = 0; - - /** produce a Gaussian factor graph containing all Hessian Factors, scaled by lambda, - * corresponding to this constraint */ - virtual GaussianFactor::shared_ptr multipliedHessian( - const Values& x, const VectorValues& duals) const { - if (!this->active(x)) { - return GaussianFactor::shared_ptr(); - } - const X1& x1 = x.at < X1 > (Base::keys_[0]); - const X2& x2 = x.at < X2 > (Base::keys_[1]); - const X3& x3 = x.at < X3 > (Base::keys_[2]); - const Vector& lambda = duals.at(this->dualKey()); - - std::vector G11, G12, G13, G22, G23, G33; - evaluateHessians(x1, x2, x3, G11, G12, G13, G22, G23, G33); - - if (lambda.size() != G11.size() || lambda.size() != G12.size() - || lambda.size() != G13.size() || lambda.size() != G22.size() - || lambda.size() != G23.size() || lambda.size() != G33.size()) { - throw std::runtime_error( - "Error in evaluateHessians: the number of returned Gij matrices must be the same as the constraint dimension!"); - } - - // Combine the Lagrange-multiplier part into this constraint factor - Matrix lG11sum = Matrix::Zero(G11[0].rows(), G11[0].cols()), lG12sum = - Matrix::Zero(G12[0].rows(), G12[0].cols()), lG13sum = Matrix::Zero( - G13[0].rows(), G13[0].cols()), lG22sum = Matrix::Zero(G22[0].rows(), - G22[0].cols()), lG23sum = Matrix::Zero(G23[0].rows(), G23[0].cols()), - lG33sum = Matrix::Zero(G33[0].rows(), - G33[0].cols()); - for (size_t i = 0; i < lambda.rows(); ++i) { - lG11sum += -lambda[i] * G11[i]; - lG12sum += -lambda[i] * G12[i]; - lG13sum += -lambda[i] * G13[i]; - lG22sum += -lambda[i] * G22[i]; - lG23sum += -lambda[i] * G23[i]; - lG33sum += -lambda[i] * G33[i]; - } - - return boost::shared_ptr( - new HessianFactor(Base::keys_[0], Base::keys_[1], Base::keys_[2], - lG11sum, lG12sum, lG13sum, Vector::Zero(X1Dim), lG22sum, lG23sum, - Vector::Zero(X2Dim), lG33sum, Vector::Zero(X3Dim), 0.0)); - } - - /** - * Default Hessian computation using numerical derivatives - * - * As an example, assuming we have f(x1,x2,x3) where dim(f) = 2, dim(x1) = 3, dim(x2) = 2, dim(x3) = 1 - * - * The Jacobian is: - * f1x1 f1x1 f1x1 | f1x2 f1x2 | f1x3 - * f2x1 f2x1 f2x1 | f2x2 f2x2 | f2x3 - * - * We transpose it to have the gradients: - * f1x1 f2x1 - * f1x1 f2x1 - * f1x1 f2x1 - * f1x2 f2x2 - * f1x2 f2x2 - * f1x3 f2x3 - * Then we vectorize this gradient to have: - * [f1x1 - * f1x1 - * f1x1 - * f1x2 - * f1x2 - * f1x3 - * f2x1 - * f2x1 - * f2x1 - * f2x2 - * f2x2 - * f2x3] - * - * The Derivative of this gradient is then: - * [f1x1x1 f1x1x1 f1x1x1 | f1x1x2 f1x1x2 | f1x1x3 - * f1x1x1 f1x1x1 f1x1x1 | f1x1x2 f1x1x2 | f1x1x3 - * f1x1x1 f1x1x1 f1x1x1 | f1x1x2 f1x1x2 | f1x1x3 - * ---------------------|---------------|------- - * f1x2x1 f1x2x1 f1x2x1 | f1x2x2 f1x2x2 | f1x2x3 - * f1x2x1 f1x2x1 f1x2x1 | f1x2x2 f1x2x2 | f1x2x3 - * ---------------------|---------------|------- - * f1x3x1 f1x3x1 f1x3x1 | f1x3x2 f1x3x2 | f1x3x3 - * ============================================= - * f2x1x1 f2x1x1 f2x1x1 | f2x1x2 f2x1x2 | f2x1x3 - * f2x1x1 f2x1x1 f2x1x1 | f2x1x2 f2x1x2 | f2x1x3 - * f2x1x1 f2x1x1 f2x1x1 | f2x1x2 f2x1x2 | f2x1x3 - * ---------------------|---------------|------- - * f2x2x1 f2x2x1 f2x2x1 | f2x2x2 f2x2x2 | f2x2x3 - * f2x2x1 f2x2x1 f2x2x1 | f2x2x2 f2x2x2 | f2x2x3 - * ---------------------|---------------|------- - * f2x3x1 f2x3x1 f2x3x1 | f2x3x2 f2x3x2 | f2x3x3 ] - * - * It is the combination of the Hessian of each component of f - * stacking on top of each other. - * - * */ - virtual void evaluateHessians(const X1& x1, const X2& x2, const X3& x3, - std::vector& G11, std::vector& G12, - std::vector& G13, std::vector& G22, - std::vector& G23, std::vector& G33) const { - - static const bool debug = false; - - boost::function vecH1( - boost::bind(&This::vectorizeH1t, this, _1, _2, _3)), vecH2( - boost::bind(&This::vectorizeH2t, this, _1, _2, _3)), vecH3( - boost::bind(&This::vectorizeH3t, this, _1, _2, _3)); - - Matrix G11all = numericalDerivative31(vecH1, x1, x2, x3, 1e-5); - Matrix G12all = numericalDerivative32(vecH1, x1, x2, x3, 1e-5); - Matrix G13all = numericalDerivative33(vecH1, x1, x2, x3, 1e-5); - Matrix G22all = numericalDerivative32(vecH2, x1, x2, x3, 1e-5); - Matrix G23all = numericalDerivative33(vecH2, x1, x2, x3, 1e-5); - Matrix G33all = numericalDerivative33(vecH3, x1, x2, x3, 1e-5); - - if (debug) { - gtsam::print(G11all, "G11all: "); - gtsam::print(G12all, "G12all: "); - gtsam::print(G13all, "G13all: "); - gtsam::print(G22all, "G22all: "); - gtsam::print(G23all, "G23all: "); - gtsam::print(G33all, "G33all: "); - std::cout << "x1dim: " << X1Dim << std::endl; - std::cout << "x2dim: " << X2Dim << std::endl; - std::cout << "x3dim: " << X3Dim << std::endl; - } - - for (size_t i = 0; i < Base::get_noiseModel()->dim(); ++i) { - G11.push_back(G11all.block(i * X1Dim, 0, X1Dim, X1Dim)); - if (debug) - gtsam::print(G11[i], "G11: "); - - G12.push_back(G12all.block(i * X1Dim, 0, X1Dim, X2Dim)); - if (debug) - gtsam::print(G12[i], "G12: "); - - G13.push_back(G13all.block(i * X1Dim, 0, X1Dim, X3Dim)); - if (debug) - gtsam::print(G13[i], "G13: "); - - G22.push_back(G22all.block(i * X2Dim, 0, X2Dim, X2Dim)); - if (debug) - gtsam::print(G22[i], "G22: "); - - G23.push_back(G23all.block(i * X2Dim, 0, X2Dim, X3Dim)); - if (debug) - gtsam::print(G23[i], "G23: "); - - G33.push_back(G33all.block(i * X3Dim, 0, X3Dim, X3Dim)); - if (debug) - gtsam::print(G33[i], "G33: "); - } - } - -private: - /** Vectorize the transpose of Jacobian H1 to compute the Hessian numerically */ - Vector vectorizeH1t(const X1& x1, const X2& x2, const X3& x3) const { - Matrix H1; - Vector err = evaluateError(x1, x2, x3, H1); - Matrix H1t = H1.transpose(); - H1t.resize(H1t.size(), 1); - return Vector(H1t); - } - - /** Vectorize the transpose of Jacobian H2 to compute the Hessian numerically */ - Vector vectorizeH2t(const X1& x1, const X2& x2, const X3& x3) const { - Matrix H2; - Vector err = evaluateError(x1, x2, x3, boost::none, H2); - Matrix H2t = H2.transpose(); - H2t.resize(H2t.size(), 1); - return Vector(H2t); - } - - /** Vectorize the transpose of Jacobian H3 to compute the Hessian numerically */ - Vector vectorizeH3t(const X1& x1, const X2& x2, const X3& x3) const { - Matrix H3; - Vector err = evaluateError(x1, x2, x3, boost::none, boost::none, H3); - Matrix H3t = H3.transpose(); - H3t.resize(H3t.size(), 1); - return Vector(H3t); - } - - /** 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 NonlinearConstraint3 - -} /* namespace gtsam */ diff --git a/gtsam_unstable/nonlinear/tests/CMakeLists.txt b/gtsam_unstable/nonlinear/tests/CMakeLists.txt index 04b560bab..ad14fcd9b 100644 --- a/gtsam_unstable/nonlinear/tests/CMakeLists.txt +++ b/gtsam_unstable/nonlinear/tests/CMakeLists.txt @@ -1 +1 @@ -gtsamAddTestsGlob(nonlinear_unstable "test*.cpp" "testCustomChartExpression.cpp;testLCNLPSolver2.cpp" "gtsam_unstable") +gtsamAddTestsGlob(nonlinear_unstable "test*.cpp" "testCustomChartExpression.cpp" "gtsam_unstable") diff --git a/gtsam_unstable/nonlinear/tests/testLCNLPSolver2.cpp b/gtsam_unstable/nonlinear/tests/testLCNLPSolver2.cpp deleted file mode 100644 index 88de347e3..000000000 --- a/gtsam_unstable/nonlinear/tests/testLCNLPSolver2.cpp +++ /dev/null @@ -1,335 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testQPSimple.cpp - * @brief Unit tests for testQPSimple - * @author Duy-Nguyen Ta - * @author Krunal Chande - * @author Luca Carlone - * @date Dec 15, 2014 - */ - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam::symbol_shorthand; -using namespace gtsam; -const double tol = 1e-10; - -//****************************************************************************** -const size_t X_AXIS = 0; -const size_t Y_AXIS = 1; -const size_t Z_AXIS = 2; - -/** - * Inequality boundary constraint on one axis (x, y or z) - * axis <= bound - */ -class AxisUpperBound : public NonlinearInequality1 { - typedef NonlinearInequality1 Base; - size_t axis_; - double bound_; - -public: - AxisUpperBound(Key key, size_t axis, double bound, Key dualKey) : Base(key, dualKey), axis_(axis), bound_(bound) { - } - - double computeError(const Pose3& pose, boost::optional H = boost::none) const { - if (H) - *H = (Matrix(1,6) << zeros(1,3), pose.rotation().matrix().row(axis_)).finished(); - return pose.translation().vector()[axis_] - bound_; - } -}; - -/** - * Inequality boundary constraint on one axis (x, y or z) - * bound <= axis - */ -class AxisLowerBound : public NonlinearInequality1 { - typedef NonlinearInequality1 Base; - size_t axis_; - double bound_; - -public: - AxisLowerBound(Key key, size_t axis, double bound, Key dualKey) : Base(key, dualKey), axis_(axis), bound_(bound) { - } - - double computeError(const Pose3& pose, boost::optional H = boost::none) const { - if (H) - *H = (Matrix(1,6) << zeros(1,3), -pose.rotation().matrix().row(axis_)).finished(); - return -pose.translation().vector()[axis_] + bound_; - } -}; - -//****************************************************************************** -TEST(testlcnlpSolver, poseWithABoundary) { - const Key dualKey = 0; - - //Instantiate LCNLP - LCNLP lcnlp; - lcnlp.cost.add(PriorFactor(X(1), Pose3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 0, 0)), noiseModel::Unit::Create(6))); - AxisUpperBound constraint(X(1), X_AXIS, 0, dualKey); - lcnlp.linearInequalities.add(constraint); - - Values initialValues; - initialValues.insert(X(1), Pose3(Rot3::ypr(0.3, 0.2, 0.3), Point3(1, 0, 0))); - - Values expectedSolution; - expectedSolution.insert(X(1), Pose3(Rot3::ypr(0.1, 0.2, 0.3), Point3(0, 0, 0))); - - // Instantiate LCNLPSolver - LCNLPSolver lcnlpSolver(lcnlp); - Values actualSolution = lcnlpSolver.optimize(initialValues).first; - - CHECK(assert_equal(expectedSolution, actualSolution, 1e-10)); -} - -//****************************************************************************** -TEST(testlcnlpSolver, poseWithNoConstraints) { - - //Instantiate LCNLP - LCNLP lcnlp; - lcnlp.cost.add(PriorFactor(X(1), Pose3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 0, 0)), noiseModel::Unit::Create(6))); - - Values initialValues; - initialValues.insert(X(1), Pose3(Rot3::ypr(0.3, 0.2, 0.3), Point3(1, 0, 0))); - - Values expectedSolution; - expectedSolution.insert(X(1), Pose3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 0, 0))); - - // Instantiate LCNLPSolver - LCNLPSolver lcnlpSolver(lcnlp); - Values actualSolution = lcnlpSolver.optimize(initialValues, true, false).first; // TODO: Fails without warmstart - - CHECK(assert_equal(expectedSolution, actualSolution, 1e-10)); -} - -//****************************************************************************** -TEST(testlcnlpSolver, poseWithinA2DBox) { - const Key dualKey = 0; - - //Instantiate LCNLP - LCNLP lcnlp; - lcnlp.cost.add(PriorFactor(X(1), Pose3(Rot3::ypr(0.1, 0.2, 0.3), Point3(10, 0.5, 0)), noiseModel::Unit::Create(6))); - lcnlp.linearInequalities.add(AxisLowerBound(X(1), X_AXIS, -1, dualKey)); // -1 <= x - lcnlp.linearInequalities.add(AxisUpperBound(X(1), X_AXIS, 1, dualKey+1)); // x <= 1 - lcnlp.linearInequalities.add(AxisLowerBound(X(1), Y_AXIS, -1, dualKey+2)); // -1 <= y - lcnlp.linearInequalities.add(AxisUpperBound(X(1), Y_AXIS, 1, dualKey+3));// y <= 1 - - Values initialValues; - initialValues.insert(X(1), Pose3(Rot3::ypr(1, -1, 2), Point3(3, -5, 0))); - - Values expectedSolution; - expectedSolution.insert(X(1), Pose3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 0.5, 0))); - - // Instantiate LCNLPSolver - LCNLPSolver lcnlpSolver(lcnlp); - Values actualSolution = lcnlpSolver.optimize(initialValues).first; - - CHECK(assert_equal(expectedSolution, actualSolution, 1e-10)); -} - -//****************************************************************************** -TEST(testlcnlpSolver, posesInA2DBox) { - const double xLowerBound = -3.0, - xUpperBound = 5.0, - yLowerBound = -1.0, - yUpperBound = 2.0, - zLowerBound = 0.0, - zUpperBound = 2.0; - - //Instantiate LCNLP - LCNLP lcnlp; - - // prior on the first pose - SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas( - (Vector(6) << 0.001, 0.001, 0.001, 0.0001, 0.0001, 0.0001).finished()); - lcnlp.cost.add(PriorFactor(X(1), Pose3(), priorNoise)); - - // odometry between factor for subsequent poses - SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas( - (Vector(6) << 0.001, 0.001, 0.001, 0.1, 0.1, 0.1).finished()); - Pose3 odo12(Rot3::ypr(M_PI/2.0, 0, 0), Point3(10, 0, 0)); - lcnlp.cost.add(BetweenFactor(X(1), X(2), odo12, odoNoise)); - - Pose3 odo23(Rot3::ypr(M_PI/2.0, 0, 0), Point3(2, 0, 2)); - lcnlp.cost.add(BetweenFactor(X(2), X(3), odo23, odoNoise)); - - // Box constraints - Key dualKey = 0; - for (size_t i=1; i<=3; ++i) { - lcnlp.linearInequalities.add(AxisLowerBound(X(i), X_AXIS, xLowerBound, dualKey++)); - lcnlp.linearInequalities.add(AxisUpperBound(X(i), X_AXIS, xUpperBound, dualKey++)); - lcnlp.linearInequalities.add(AxisLowerBound(X(i), Y_AXIS, yLowerBound, dualKey++)); - lcnlp.linearInequalities.add(AxisUpperBound(X(i), Y_AXIS, yUpperBound, dualKey++)); - lcnlp.linearInequalities.add(AxisLowerBound(X(i), Z_AXIS, zLowerBound, dualKey++)); - lcnlp.linearInequalities.add(AxisUpperBound(X(i), Z_AXIS, zUpperBound, dualKey++)); - } - - Values initialValues; - initialValues.insert(X(1), Pose3(Rot3(), Point3(0, 0, 0))); - initialValues.insert(X(2), Pose3(Rot3(), Point3(0, 0, 0))); - initialValues.insert(X(3), Pose3(Rot3(), Point3(0, 0, 0))); - - - Values expectedSolution; - expectedSolution.insert(X(1), Pose3()); - expectedSolution.insert(X(2), Pose3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(5, 0, 0))); - expectedSolution.insert(X(3), Pose3(Rot3::ypr(M_PI, 0, 0), Point3(5, 2, 2))); - - // Instantiate LCNLPSolver - LCNLPSolver lcnlpSolver(lcnlp); - bool useWarmStart = true; - Values actualSolution = lcnlpSolver.optimize(initialValues, useWarmStart).first; - -// cout << "Rotation angles: " << endl; -// for (size_t i = 1; i<=3; i++) { -// cout << actualSolution.at(X(i)).rotation().ypr().transpose()*180/M_PI << endl; -// } - -// cout << "Actual Error: " << lcnlp.cost.error(actualSolution) << endl; -// cout << "Expected Error: " << lcnlp.cost.error(expectedSolution) << endl; -// actualSolution.print("actualSolution: "); - - AxisLowerBound factor(X(1), X_AXIS, xLowerBound, dualKey++); - Matrix hessian = numericalHessian(boost::bind(&AxisLowerBound::computeError, factor, _1, boost::none), Pose3(), 1e-3); - cout << "Hessian: \n" << hessian << endl; - CHECK(assert_equal(expectedSolution, actualSolution, 1e-5)); -} - -//****************************************************************************** -TEST(testlcnlpSolver, iterativePoseinBox) { - const double xLowerBound = -1.0, - xUpperBound = 1.0, - yLowerBound = -1.0, - yUpperBound = 1.0, - zLowerBound = -1.0, - zUpperBound = 1.0; - - //Instantiate LCNLP - LCNLP lcnlp; - - // prior on the first pose - SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas( - (Vector(6) << 0.001, 0.001, 0.001, 0.0001, 0.0001, 0.0001).finished()); - lcnlp.cost.add(PriorFactor(X(0), Pose3(), priorNoise)); - - // odometry between factor for subsequent poses - SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas( - (Vector(6) << 0.001, 0.001, 0.001, 0.1, 0.1, 0.1).finished()); - Pose3 odo(Rot3::ypr(0, 0, 0), Point3(0.4, 0, 0)); - - Values initialValues; - Values isamValues; - initialValues.insert(X(0), Pose3(Rot3(), Point3(0, 0, 0))); - isamValues.insert(X(0), Pose3(Rot3(), Point3(0, 0, 0))); - std::pair solutionAndDuals; - solutionAndDuals.first.insert(X(0), Pose3(Rot3(), Point3(0, 0, 0))); - Values actualSolution; - bool useWarmStart = true; - bool firstTime = true; - - // Box constraints - Key dualKey = 0; - for (size_t i=1; i<=4; ++i) { - - lcnlp.cost.add(BetweenFactor(X(i-1), X(i), odo, odoNoise)); - lcnlp.linearInequalities.add(AxisLowerBound(X(i), X_AXIS, xLowerBound, dualKey++)); - lcnlp.linearInequalities.add(AxisUpperBound(X(i), X_AXIS, xUpperBound, dualKey++)); - lcnlp.linearInequalities.add(AxisLowerBound(X(i), Y_AXIS, yLowerBound, dualKey++)); - lcnlp.linearInequalities.add(AxisUpperBound(X(i), Y_AXIS, yUpperBound, dualKey++)); - lcnlp.linearInequalities.add(AxisLowerBound(X(i), Z_AXIS, zLowerBound, dualKey++)); - lcnlp.linearInequalities.add(AxisUpperBound(X(i), Z_AXIS, zUpperBound, dualKey++)); - - initialValues.insert(X(i), Pose3(Rot3(), Point3(0, 0, 0))); - isamValues = solutionAndDuals.first; - isamValues.insert(X(i), solutionAndDuals.first.at(X(i-1))); - isamValues.print("iSAM values: \n"); - - - // Instantiate LCNLPSolver - LCNLPSolver lcnlpSolver(lcnlp); -// if (firstTime) { - solutionAndDuals = lcnlpSolver.optimize(isamValues, useWarmStart); -// firstTime = false; -// } -// else { -// cout << " using this \n "; -// solutionAndDuals = lcnlpSolver.optimize(isamValues, solutionAndDuals.second, useWarmStart); -// -// } - cout << " ************************** \n"; - } - actualSolution = solutionAndDuals.first; - cout << "out of loop" << endl; - Values expectedSolution; - expectedSolution.insert(X(0), Pose3()); - expectedSolution.insert(X(1), Pose3(Rot3::ypr(0, 0, 0), Point3(0.25, 0, 0))); - expectedSolution.insert(X(2), Pose3(Rot3::ypr(0, 0, 0), Point3(0.50, 0, 0))); - expectedSolution.insert(X(3), Pose3(Rot3::ypr(0, 0, 0), Point3(0.75, 0, 0))); - expectedSolution.insert(X(4), Pose3(Rot3::ypr(0, 0, 0), Point3(1, 0, 0))); - - - -// cout << "Rotation angles: " << endl; -// for (size_t i = 1; i<=3; i++) { -// cout << actualSolution.at(X(i)).rotation().ypr().transpose()*180/M_PI << endl; -// } - - cout << "Actual Error: " << lcnlp.cost.error(actualSolution) << endl; - cout << "Expected Error: " << lcnlp.cost.error(expectedSolution) << endl; - actualSolution.print("actualSolution: "); - - CHECK(assert_equal(expectedSolution, actualSolution, 1e-5)); -} - - -//****************************************************************************** -double testHessian(const Pose3& X) { - return X.transform_from(Point3(0,0,0)).x(); -} - -Pose3 pose(Rot3::ypr(0.1, 0.4, 0.7), Point3(1, 9.5, 7.3)); -TEST(testlcnlpSolver, testingHessian) { - Matrix H = numericalHessian(testHessian, pose, 1e-5); -} - -double h3(const Vector6& v) { - return pose.retract(v).translation().x(); -} - -TEST(testlcnlpSolver, NumericalHessian3) { - Matrix6 expected; - expected.setZero(); - Vector6 z; - z.setZero(); - EXPECT(assert_equal(expected, numericalHessian(h3, z, 1e-5), 1e-5)); -} - -//****************************************************************************** -int main() { - cout<<"here: "< -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam::symbol_shorthand; -using namespace gtsam; -const double tol = 1e-10; - -//****************************************************************************** -// x + y - 1 = 0 -class ConstraintProblem1: public LinearEqualityFactor2 { - typedef LinearEqualityFactor2 Base; - -public: - ConstraintProblem1(Key xK, Key yK, Key dualKey) : - Base(xK, yK, dualKey, 1) { - } - - // x + y - 1 - Vector evaluateError(const double& x, const double& y, - boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { - if (H1) - *H1 = I_1x1; - if (H2) - *H2 = I_1x1; - return (Vector(1) << x + y - 1.0).finished(); - } -}; - -TEST(testlcnlpSolver, QPProblem) { - const Key dualKey = 0; - - // Simple quadratic cost: x1^2 + x2^2 - // Note the Hessian encodes: - // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f - // Hence here we have G11 = 2, G12 = 0, G22 = 2, g1 = 0, g2 = 0, f = 0 - HessianFactor hf(X(1), Y(1), 2.0 * Matrix::Ones(1,1), Vector::Zero(1), Vector::Zero(1), - 2*Matrix::Ones(1,1), Vector::Zero(1) , 0); - - EqualityFactorGraph equalities; - LinearEquality linearConstraint(X(1), Vector::Ones(1), Y(1), Vector::Ones(1), Vector::Ones(1), dualKey);// x + y - 1 = 0 - equalities.push_back(linearConstraint); - - // Compare against QP - QP qp; - qp.cost.add(hf); - qp.equalities = equalities; - - // instantiate QPsolver - QPSolver qpSolver(qp); - // create initial values for optimization - VectorValues initialVectorValues; - initialVectorValues.insert(X(1), Vector::Zero(1)); - initialVectorValues.insert(Y(1), Vector::Ones(1)); - VectorValues expectedSolution = qpSolver.optimize(initialVectorValues).first; - - //Instantiate LinearConstraintNLP - LinearConstraintNLP lcnlp; - Values linPoint; - linPoint.insert(X(1), Vector::Zero(1)); - linPoint.insert(Y(1), Vector::Zero(1)); - lcnlp.cost.add(LinearContainerFactor(hf, linPoint));// wrap it using linearcontainerfactor - lcnlp.linearEqualities.add(ConstraintProblem1(X(1), Y(1), dualKey)); - - Values initialValues; - initialValues.insert(X(1), 0.0); - initialValues.insert(Y(1), 0.0); - - // Instantiate LinearConstraintSQP - LinearConstraintSQP lcnlpSolver(lcnlp); - Values actualValues = lcnlpSolver.optimize(initialValues).first; - - DOUBLES_EQUAL(expectedSolution.at(X(1))[0], actualValues.at(X(1)), 1e-100); - DOUBLES_EQUAL(expectedSolution.at(Y(1))[0], actualValues.at(Y(1)), 1e-100); - -} - -//****************************************************************************** -/** - * A simple linear constraint on Pose3's x coordinate enforcing x==0 - */ -class LineConstraintX: public LinearEqualityFactor1 { - typedef LinearEqualityFactor1 Base; -public: - LineConstraintX(Key key, Key dualKey) : - Base(key, dualKey, 1) { - } - - Vector evaluateError(const Pose3& pose, boost::optional H = - boost::none) const { - if (H) - *H = - (Matrix(1, 6) << Matrix::Zero(1, 3), pose.rotation().matrix().row(0)).finished(); - return (Vector(1) << pose.x()).finished(); - } -}; - -TEST(testlcnlpSolver, poseOnALine) { - const Key dualKey = 0; - - //Instantiate LinearConstraintNLP - LinearConstraintNLP lcnlp; - lcnlp.cost.add(PriorFactor(X(1), Pose3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(1, 0, 0)), noiseModel::Unit::Create(6))); - LineConstraintX constraint(X(1), dualKey); - lcnlp.linearEqualities.add(constraint); - - Values initialValues; - initialValues.insert(X(1), Pose3(Rot3::Ypr(0.3, 0.2, 0.3), Point3(1,0,0))); - - Values expectedSolution; - expectedSolution.insert(X(1), Pose3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(0,0,0))); - - // Instantiate LinearConstraintSQP - LinearConstraintSQP lcnlpSolver(lcnlp); - Values actualSolution = lcnlpSolver.optimize(initialValues).first; - - CHECK(assert_equal(expectedSolution, actualSolution, 1e-10)); -} - -//****************************************************************************** -/// x + y - 1 <= 0 -class InequalityProblem1: public LinearInequalityFactor2 { - typedef LinearInequalityFactor2 Base; -public: - InequalityProblem1(Key xK, Key yK, Key dualKey) : - Base(xK, yK, dualKey) { - } - - double computeError(const double& x, const double& y, - boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { - if (H1) - *H1 = I_1x1; - if (H2) - *H2 = I_1x1; - return x + y - 1.0; - } -}; - -TEST(testlcnlpSolver, inequalityConstraint) { - const Key dualKey = 0; - - // Simple quadratic cost: x^2 + y^2 - // Note the Hessian encodes: - // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f - // Hence here we have G11 = 2, G12 = 0, G22 = 2, g1 = 0, g2 = 0, f = 0 - HessianFactor hf(X(1), Y(1), 2.0 * Matrix::Ones(1,1), Vector::Zero(1), Vector::Zero(1), - 2*Matrix::Ones(1,1), Vector::Zero(1) , 0); - - InequalityFactorGraph inequalities; - LinearInequality linearConstraint(X(1), Vector::Ones(1), Y(1), Vector::Ones(1), 1.0, dualKey);// x + y - 1 <= 0 - inequalities.push_back(linearConstraint); - - // Compare against QP - QP qp; - qp.cost.add(hf); - qp.inequalities = inequalities; - - // instantiate QPsolver - QPSolver qpSolver(qp); - // create initial values for optimization - VectorValues initialVectorValues; - initialVectorValues.insert(X(1), Vector::Zero(1)); - initialVectorValues.insert(Y(1), Vector::Zero(1)); - VectorValues expectedSolution = qpSolver.optimize(initialVectorValues).first; - - //Instantiate LinearConstraintNLP - LinearConstraintNLP lcnlp; - Values linPoint; - linPoint.insert(X(1), Vector::Zero(1)); - linPoint.insert(Y(1), Vector::Zero(1)); - lcnlp.cost.add(LinearContainerFactor(hf, linPoint));// wrap it using linearcontainerfactor - lcnlp.linearInequalities.add(InequalityProblem1(X(1), Y(1), dualKey)); - - Values initialValues; - initialValues.insert(X(1), 1.0); - initialValues.insert(Y(1), -10.0); - - // Instantiate LinearConstraintSQP - LinearConstraintSQP lcnlpSolver(lcnlp); - Values actualValues = lcnlpSolver.optimize(initialValues).first; - - DOUBLES_EQUAL(expectedSolution.at(X(1))[0], actualValues.at(X(1)), 1e-10); - DOUBLES_EQUAL(expectedSolution.at(Y(1))[0], actualValues.at(Y(1)), 1e-10); -} - -//****************************************************************************** -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -//****************************************************************************** diff --git a/gtsam_unstable/nonlinear/tests/testLinearInequalityFactorGraph.cpp b/gtsam_unstable/nonlinear/tests/testLinearInequalityFactorGraph.cpp deleted file mode 100644 index 233ee213b..000000000 --- a/gtsam_unstable/nonlinear/tests/testLinearInequalityFactorGraph.cpp +++ /dev/null @@ -1,42 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testLinearInequalityFactorGraph.cpp - * @author Duy-Nguyen Ta - * @author Krunal Chande - * @author Luca Carlone - * @date Dec 15, 2014 - */ - -#include -#include -#include -#include - -using namespace std; -using namespace gtsam::symbol_shorthand; -using namespace gtsam; -const double tol = 1e-10; - -//****************************************************************************** -TEST(LinearInequalityFactorGraph, constructor) { - LinearInequalityFactorGraph linearInequalities; - CHECK(linearInequalities.empty()); -} - - -//****************************************************************************** -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -//****************************************************************************** diff --git a/gtsam_unstable/nonlinear/tests/testLinearInequalityManifolds.cpp b/gtsam_unstable/nonlinear/tests/testLinearInequalityManifolds.cpp deleted file mode 100644 index 3d6c401b0..000000000 --- a/gtsam_unstable/nonlinear/tests/testLinearInequalityManifolds.cpp +++ /dev/null @@ -1,68 +0,0 @@ -///* ---------------------------------------------------------------------------- -// -// * 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 testLinearInequalityManifolds.cpp -// * @brief Unit tests for testLinearInequalityManifolds -// * @author Krunal Chande -// * @author Duy-Nguyen Ta -// * @author Luca Carlone -// * @date Dec 15, 2014 -// */ -// -//#include -//#include -//#include -// -//using namespace std; -//using namespace gtsam::symbol_shorthand; -//using namespace gtsam; -//const double tol = 1e-10; -// -//namespace gtsam { -//class PositionConstraint : public LinearInequalityManifold1 { -//public: -// PositionConstraint() -//}; -//} -////****************************************************************************** -//TEST(testLinearEqualityManifolds, equals ) { -// // Instantiate a class LinearEqualityManifolds -// LinearEqualityManifolds le1(); -// -// // Instantiate another class LinearEqualityManifolds -// LinearEqualityManifolds le2(); -// -// // check equals -// CHECK(assert_equal(le1,le1)); -// CHECK(le2.equals(le2)); -// CHECK(!le2.equals(le1)); -// CHECK(!le1.equals(le2)); -//} -// -////****************************************************************************** -//TEST(testLinearEqualityManifolds, linearize ) { -//} -// -////****************************************************************************** -//TEST(testLinearEqualityManifolds, size ) { -//} -// -////****************************************************************************** -//int main() { -// TestResult tr; -// return TestRegistry::runAllTests(tr); -//} -//****************************************************************************** - -int main() { - -} diff --git a/gtsam_unstable/nonlinear/tests/testNonlinearConstraints.cpp b/gtsam_unstable/nonlinear/tests/testNonlinearConstraints.cpp deleted file mode 100644 index 40cd6f3a7..000000000 --- a/gtsam_unstable/nonlinear/tests/testNonlinearConstraints.cpp +++ /dev/null @@ -1,176 +0,0 @@ -/** - * @file testNonlinearConstraints.cpp - * @brief - * @author Duy-Nguyen Ta - * @date Oct 26, 2013 - */ - -#include -#include -#include -#include -#include -#include "../NonlinearConstraint.h" - -using namespace gtsam; -using namespace gtsam::symbol_shorthand; -using namespace std; - -TEST(Vector, vectorize) { - Matrix m = (Matrix(4,3) << 1,2,3,4,5,6,7,8,9,10,11,12).finished(); - Matrix m2 = m.transpose(); - m2.resize(12,1); - Vector v = Vector(m2); - Vector expectedV = (Vector(12) << 1,2,3,4,5,6,7,8,9,10,11,12).finished(); - EXPECT(assert_equal(expectedV, v)); -} - -/* ************************************************************************* */ -/** - * Test 3-way equality nonlinear constraint - * x(1) + x(2)^2 + x(3)^3 = 3 - */ -class Constraint: public NonlinearConstraint3 { - typedef NonlinearConstraint3 Base; - -public: - Constraint(Key key1, Key key2, Key key3, Key dualKey) : - Base(key1, key2, key3, dualKey, 1) { - } - - Vector evaluateError(const LieScalar& x1, const LieScalar& x2, - const LieScalar& x3, boost::optional H1 = boost::none, - boost::optional H2 = boost::none, boost::optional H3 = - boost::none) const { - if (H1) { - *H1 = (Matrix(1, 1) << 1.0).finished(); - } - if (H2) { - *H2 = (Matrix(1, 1) << 2.0 * x2.value()).finished(); - } - if (H3) { - *H3 = (Matrix(1, 1) << 3.0 * x3.value() * x3.value()).finished(); - } - - return (Vector(1) << - x1.value() + x2.value() * x2.value() - + x3.value() * x3.value() * x3.value() - 3.0).finished(); - } - - void expectedHessians(const LieScalar& x1, const LieScalar& x2, - const LieScalar& x3, std::vector& G11, std::vector& G12, - std::vector& G13, std::vector& G22, - std::vector& G23, std::vector& G33) const { - G11.push_back(Matrix::Zero(1, 1)); - G12.push_back(Matrix::Zero(1, 1)); - G13.push_back(Matrix::Zero(1, 1)); - G22.push_back((Matrix(1, 1) << 2.0).finished()); - G23.push_back(Matrix::Zero(1, 1)); - G33.push_back((Matrix(1, 1) << 6.0 * x3.value()).finished()); - } -}; - -/* ************************************************************************* */ - -TEST(NonlinearConstraint3, Hessian) { - LieScalar x1(2.0), x2(sqrt(2) - 1), x3(sqrt(2) - 1), x4(2.0), x5(0.5); - Constraint constraint(X(1), X(2), X(3), 0); - - std::vector expectedG11, expectedG12, expectedG13, expectedG22, - expectedG23, expectedG33; - constraint.expectedHessians(x1, x2, x3, expectedG11, expectedG12, expectedG13, - expectedG22, expectedG23, expectedG33); - std::vector G11, G12, G13, G22, G23, G33; - constraint.evaluateHessians(x1, x2, x3, G11, G12, G13, G22, G23, G33); - EXPECT(assert_equal(expectedG11[0], G11[0], 1e-5)); - EXPECT(assert_equal(expectedG12[0], G12[0], 1e-5)); - EXPECT(assert_equal(expectedG13[0], G13[0], 1e-5)); - EXPECT(assert_equal(expectedG22[0], G22[0], 1e-5)); - EXPECT(assert_equal(expectedG23[0], G23[0], 1e-5)); - EXPECT(assert_equal(expectedG33[0], G33[0], 1e-5)); -} - -/* ************************************************************************* */ -/** - * Test 3-way nonlinear equality multi-values constraint - * p(1).x + p(2).x^2 + p(3).y^3 = 3 - * p(1).x^2 + p(2).y + p(3).x^3 = 5 - */ -class Constraint2d: public NonlinearConstraint3 { - typedef NonlinearConstraint3 Base; - -public: - Constraint2d(Key key1, Key key2, Key key3, Key dualKey) : - Base(key1, key2, key3, dualKey, 1) { - } - - Vector evaluateError(const Point2& p1, const Point2& p2, const Point2& p3, - boost::optional H1 = boost::none, boost::optional H2 = - boost::none, boost::optional H3 = boost::none) const { - if (H1) { - *H1 = (Matrix(2, 2) << 1.0, 0.0, 2 * p1.x(), 0.0).finished(); - } - if (H2) { - *H2 = (Matrix(2, 2) << 2.0 * p2.x(), 0.0, 0.0, 1.0).finished(); - } - if (H3) { - *H3 = (Matrix(2, 2) << 0.0, 3.0 * p3.y() * p3.y(), 3.0 * p3.x() * p3.x(), 0.0).finished(); - } - - return (Vector(2) << p1.x() + p2.x() * p2.x() + p3.y() * p3.y() * p3.y() - 3.0, - p1.x() * p1.x() + p2.y() + p3.x() * p3.x() * p3.x() - 5.0).finished(); - } - - void expectedHessians(const Point2& x1, const Point2& x2, const Point2& x3, - std::vector& G11, std::vector& G12, - std::vector& G13, std::vector& G22, - std::vector& G23, std::vector& G33) const { - G11.push_back(Matrix::Zero(2, 2)); - G11.push_back((Matrix(2,2) << 2.0, 0.0, 0.0, 0.0).finished()); - - G12.push_back(Matrix::Zero(2, 2)); - G12.push_back(Matrix::Zero(2, 2)); - - G13.push_back(Matrix::Zero(2, 2)); - G13.push_back(Matrix::Zero(2, 2)); - - G22.push_back((Matrix(2,2) << 2.0, 0.0, 0.0, 0.0).finished()); - G22.push_back(Matrix::Zero(2, 2)); - - G23.push_back(Matrix::Zero(2, 2)); - G23.push_back(Matrix::Zero(2, 2)); - - G33.push_back((Matrix(2, 2) << 0.0, 0.0, 0.0, 6.0 * x3.y() ).finished()); - G33.push_back((Matrix(2, 2) << 6.0 * x3.x(), 0.0, 0.0, 0.0 ).finished()); - } -}; - -/* ************************************************************************* */ - -TEST(NonlinearConstraint3, Hessian2) { - Point2 x1(2.0, 2.0), x2(sqrt(2) - 1, 3.0), x3(4.0, sqrt(2) - 2); - Constraint2d constraint(X(1), X(2), X(3), 0); - - std::vector expectedG11, expectedG12, expectedG13, expectedG22, - expectedG23, expectedG33; - constraint.expectedHessians(x1, x2, x3, expectedG11, expectedG12, expectedG13, - expectedG22, expectedG23, expectedG33); - std::vector G11, G12, G13, G22, G23, G33; - constraint.evaluateHessians(x1, x2, x3, G11, G12, G13, G22, G23, G33); - for (size_t i = 0; i