12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Deleted unrealted SQP files.

release/4.3a0
= 2016-06-17 12:29:03 -04:00
parent b91e531fac
commit 41684ee6e0
16 changed files with 1 additions and 2261 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
gtsam_unstable/nonlinear/ConstrainedFactor.h
View File

@ -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 <gtsam/base/types.h>
#include <boost/shared_ptr.hpp>
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<ConstrainedFactor> 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 */

133
gtsam_unstable/nonlinear/LinearConstraintSQP.cpp
View File

@ -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 <gtsam/inference/FactorGraph-inst.h>
#include <gtsam_unstable/linear/QPSolver.h>
#include <gtsam_unstable/nonlinear/LinearConstraintSQP.h>
#include <gtsam_unstable/nonlinear/ConstrainedFactor.h>
#include <iostream>
namespace gtsam {
/* ************************************************************************* */
bool LinearConstraintSQP::isStationary(const VectorValues& delta) const {
return delta.vector().lpNorm<Eigen::Infinity>() < 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<ConstrainedFactor>(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<ConstrainedFactor>(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<Values, VectorValues> 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

135
gtsam_unstable/nonlinear/LinearConstraintSQP.h
View File

@ -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 <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizerParams.h>
#include <gtsam_unstable/nonlinear/LinearEqualityFactorGraph.h>
#include <gtsam_unstable/nonlinear/LinearInequalityFactorGraph.h>
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<Values, VectorValues> optimize(const Values& initialValues) const;
};
}

192
gtsam_unstable/nonlinear/LinearEqualityFactor.h
View File

@ -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 <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam_unstable/nonlinear/ConstrainedFactor.h>
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 VALUE>
class LinearEqualityFactor1: public NoiseModelFactor1<VALUE>,
public ConstrainedFactor {
public:
// typedefs for value types pulled from keys
typedef VALUE X;
protected:
typedef NoiseModelFactor1<VALUE> Base;
typedef LinearEqualityFactor1<VALUE> 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<Matrix&> 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 VALUE1, class VALUE2>
class LinearEqualityFactor2: public NoiseModelFactor2<VALUE1, VALUE2>,
public ConstrainedFactor {
public:
// typedefs for value types pulled from keys
typedef VALUE1 X1;
typedef VALUE2 X2;
protected:
typedef NoiseModelFactor2<VALUE1, VALUE2> Base;
typedef LinearEqualityFactor2<VALUE1, VALUE2> 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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> 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 VALUE1, class VALUE2, class VALUE3>
class LinearEqualityFactor3: public NoiseModelFactor3<VALUE1, VALUE2, VALUE3>,
public ConstrainedFactor {
public:
// typedefs for value types pulled from keys
typedef VALUE1 X1;
typedef VALUE2 X2;
typedef VALUE3 X3;
protected:
typedef NoiseModelFactor3<VALUE1, VALUE2, VALUE3> Base;
typedef LinearEqualityFactor3<VALUE1, VALUE2, VALUE3> 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<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const = 0;
};
// \class LinearEqualityFactor3
} /* namespace gtsam */

52
gtsam_unstable/nonlinear/LinearEqualityFactorGraph.cpp
View File

@ -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 <gtsam_unstable/nonlinear/LinearEqualityFactorGraph.h>
#include <gtsam_unstable/nonlinear/ConstrainedFactor.h>
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<JacobianFactor>(
factor->linearize(linearizationPoint));
ConstrainedFactor::shared_ptr constraint = boost::dynamic_pointer_cast<ConstrainedFactor>(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<NoiseModelFactor>(factor);
Vector error = noiseModelFactor->unwhitenedError(values);
if (error.lpNorm<Eigen::Infinity>() > tol)
return false;
}
return true;
}
}

45
gtsam_unstable/nonlinear/LinearEqualityFactorGraph.h
View File

@ -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 <gtsam/inference/FactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam_unstable/linear/EqualityFactorGraph.h>
namespace gtsam {
/**
* FactorGraph container for linear equality factors c(x)==0 at the nonlinear level
*/
class LinearEqualityFactorGraph: public FactorGraph<NonlinearFactor> {
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;
};
}

138
gtsam_unstable/nonlinear/LinearInequalityFactor.h
View File

@ -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 <gtsam_unstable/nonlinear/LinearEqualityFactor.h>
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 VALUE>
class LinearInequalityFactor1: public LinearEqualityFactor1<VALUE> {
public:
// typedefs for value types pulled from keys
typedef VALUE X;
protected:
typedef LinearEqualityFactor1<VALUE> Base;
typedef LinearInequalityFactor1<VALUE> 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<Matrix&> H1 = boost::none) const = 0;
/** predefine evaluateError to return a 1-dimension vector */
virtual Vector
evaluateError(const X& x, boost::optional<Matrix&> 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 VALUE1, class VALUE2>
class LinearInequalityFactor2: public LinearEqualityFactor2<VALUE1, VALUE2> {
public:
// typedefs for value types pulled from keys
typedef VALUE1 X1;
typedef VALUE2 X2;
protected:
typedef LinearEqualityFactor2<VALUE1, VALUE2> Base;
typedef LinearInequalityFactor2<VALUE1, VALUE2> 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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const = 0;
/** predefine evaluateError to return a 1-dimension vector */
virtual Vector
evaluateError(const X1& x1, const X2& x2, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
return (Vector(1) << computeError(x1, x2, H1, H2)).finished();
}
};
// \class LinearEqualityFactor2
} /* namespace gtsam */

69
gtsam_unstable/nonlinear/LinearInequalityFactorGraph.cpp
View File

@ -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 <gtsam_unstable/nonlinear/LinearInequalityFactorGraph.h>
#include <gtsam_unstable/nonlinear/ConstrainedFactor.h>
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<JacobianFactor>(
factor->linearize(linearizationPoint));
ConstrainedFactor::shared_ptr constraint
= boost::dynamic_pointer_cast<ConstrainedFactor>(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<NoiseModelFactor>(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<ConstrainedFactor>(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;
}
}

46
gtsam_unstable/nonlinear/LinearInequalityFactorGraph.h
View File

@ -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 <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam_unstable/linear/InequalityFactorGraph.h>
namespace gtsam {
/**
* FactorGraph container for linear inequality factors c(x)<=0 at the nonlinear level
*/
class LinearInequalityFactorGraph: public FactorGraph<NonlinearFactor> {
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

552
gtsam_unstable/nonlinear/NonlinearConstraint.h
View File

@ -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 <gtsam/base/Manifold.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam_unstable/nonlinear/ConstrainedFactor.h>
namespace gtsam {
/* ************************************************************************* */
/** A convenient base class for creating a nonlinear equality constraint with 1
* variables. To derive from this class, implement evaluateError(). */
template<class VALUE>
class NonlinearConstraint1: public NoiseModelFactor1<VALUE>, public ConstrainedFactor {
public:
// typedefs for value types pulled from keys
typedef VALUE X;
protected:
typedef NoiseModelFactor1<VALUE> Base;
typedef NonlinearConstraint1<VALUE> This;
private:
static const int X1Dim = traits<VALUE>::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<Matrix&> 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<Matrix> 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<Matrix>& G11) const {
static const bool debug = false;
typedef Eigen::Matrix<double, X1Dim, 1> actual_size;
boost::function<actual_size(const X&)> 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<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar
& boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this));
}
};
// \class NonlinearConstraint1
/* ************************************************************************* */
/** A convenient base class for creating your own ConstrainedFactor with 2
* variables. To derive from this class, implement evaluateError(). */
template<class VALUE1, class VALUE2>
class NonlinearConstraint2: public NoiseModelFactor2<VALUE1, VALUE2>, public ConstrainedFactor {
public:
// typedefs for value types pulled from keys
typedef VALUE1 X1;
typedef VALUE2 X2;
protected:
typedef NoiseModelFactor2<VALUE1, VALUE2> Base;
typedef NonlinearConstraint2<VALUE1, VALUE2> This;
private:
static const int X1Dim = traits<VALUE1>::dimension;
static const int X2Dim = traits<VALUE2>::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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> 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<Matrix> 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<HessianFactor>(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<Matrix>& G11, std::vector<Matrix>& G12,
std::vector<Matrix>& G22) const {
static const bool debug = false;
boost::function<Vector(const X1&, const X2&)> 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<VALUE1>::dimension << std::endl;
std::cout << "x2dim: " << traits<VALUE2>::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<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar
& boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this));
}
};
// \class NonlinearConstraint2
/* ************************************************************************* */
/** A convenient base class for creating your own ConstrainedFactor with 3
* variables. To derive from this class, implement evaluateError(). */
template<class VALUE1, class VALUE2, class VALUE3>
class NonlinearConstraint3: public NoiseModelFactor3<VALUE1, VALUE2, VALUE3>, public ConstrainedFactor {
public:
// typedefs for value types pulled from keys
typedef VALUE1 X1;
typedef VALUE2 X2;
typedef VALUE3 X3;
protected:
typedef NoiseModelFactor3<VALUE1, VALUE2, VALUE3> Base;
typedef NonlinearConstraint3<VALUE1, VALUE2, VALUE3> This;
private:
static const int X1Dim = traits<VALUE1>::dimension;
static const int X2Dim = traits<VALUE2>::dimension;
static const int X3Dim = traits<VALUE3>::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<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> 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<Matrix> 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<HessianFactor>(
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<Matrix>& G11, std::vector<Matrix>& G12,
std::vector<Matrix>& G13, std::vector<Matrix>& G22,
std::vector<Matrix>& G23, std::vector<Matrix>& G33) const {
static const bool debug = false;
boost::function<Vector(const X1&, const X2&, const X3&)> 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<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar
& boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this));
}
};
// \class NonlinearConstraint3
} /* namespace gtsam */

2
gtsam_unstable/nonlinear/tests/CMakeLists.txt
View File

@ -1 +1 @@
gtsamAddTestsGlob(nonlinear_unstable "test*.cpp" "testCustomChartExpression.cpp;testLCNLPSolver2.cpp" "gtsam_unstable") gtsamAddTestsGlob(nonlinear_unstable "test*.cpp" "testCustomChartExpression.cpp" "gtsam_unstable")

335
gtsam_unstable/nonlinear/tests/testLCNLPSolver2.cpp
View File

@ -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 <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam_unstable/linear/QPSolver.h>
#include <gtsam_unstable/nonlinear/LCNLPSolver.h>
#include <gtsam_unstable/nonlinear/NonlinearInequality.h>
#include <CppUnitLite/TestHarness.h>
#include <iostream>
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<Pose3> {
typedef NonlinearInequality1<Pose3> 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<Matrix&> 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<Pose3> {
typedef NonlinearInequality1<Pose3> 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<Matrix&> 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<Pose3>(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<Pose3>(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<Pose3>(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<Pose3>(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<Pose3>(X(1), X(2), odo12, odoNoise));
Pose3 odo23(Rot3::ypr(M_PI/2.0, 0, 0), Point3(2, 0, 2));
lcnlp.cost.add(BetweenFactor<Pose3>(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<Pose3>(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<Pose3>(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<Pose3>(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<Values, VectorValues> 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<Pose3>(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<Pose3>(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: "<<endl;
TestResult tr;
return TestRegistry::runAllTests(tr);
}
//******************************************************************************

223
gtsam_unstable/nonlinear/tests/testLinearConstraintSQP.cpp
View File

@ -1,223 +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 testLinearConstraintSQP.cpp
* @brief Unit tests for testLinearConstraintSQP
* @author Duy-Nguyen Ta
* @author Krunal Chande
* @author Luca Carlone
* @date Dec 15, 2014
*/
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam_unstable/linear/QPSolver.h>
#include <gtsam_unstable/nonlinear/LinearConstraintSQP.h>
#include <gtsam_unstable/nonlinear/LinearInequalityFactor.h>
#include <CppUnitLite/TestHarness.h>
#include <iostream>
using namespace std;
using namespace gtsam::symbol_shorthand;
using namespace gtsam;
const double tol = 1e-10;
//******************************************************************************
// x + y - 1 = 0
class ConstraintProblem1: public LinearEqualityFactor2<double, double> {
typedef LinearEqualityFactor2<double, double> 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<Matrix&> H1 = boost::none, boost::optional<Matrix&> 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<Vector1>(X(1), Vector::Zero(1));
linPoint.insert<Vector1>(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<double>(X(1)), 1e-100);
DOUBLES_EQUAL(expectedSolution.at(Y(1))[0], actualValues.at<double>(Y(1)), 1e-100);
}
//******************************************************************************
/**
* A simple linear constraint on Pose3's x coordinate enforcing x==0
*/
class LineConstraintX: public LinearEqualityFactor1<Pose3> {
typedef LinearEqualityFactor1<Pose3> Base;
public:
LineConstraintX(Key key, Key dualKey) :
Base(key, dualKey, 1) {
}
Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> 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<Pose3>(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<double, double> {
typedef LinearInequalityFactor2<double, double> Base;
public:
InequalityProblem1(Key xK, Key yK, Key dualKey) :
Base(xK, yK, dualKey) {
}
double computeError(const double& x, const double& y,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> 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<Vector1>(X(1), Vector::Zero(1));
linPoint.insert<Vector1>(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<double>(X(1)), 1e-10);
DOUBLES_EQUAL(expectedSolution.at(Y(1))[0], actualValues.at<double>(Y(1)), 1e-10);
}
//******************************************************************************
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
//******************************************************************************

42
gtsam_unstable/nonlinear/tests/testLinearInequalityFactorGraph.cpp
View File

@ -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 <gtsam/inference/Symbol.h>
#include <gtsam_unstable/nonlinear/LinearInequalityFactorGraph.h>
#include <CppUnitLite/TestHarness.h>
#include <iostream>
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);
}
//******************************************************************************

68
gtsam_unstable/nonlinear/tests/testLinearInequalityManifolds.cpp
View File

@ -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 <gtsam/inference/Symbol.h>
//#include <CppUnitLite/TestHarness.h>
//#include <iostream>
//
//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() {
}

176
gtsam_unstable/nonlinear/tests/testNonlinearConstraints.cpp
View File

@ -1,176 +0,0 @@
/**
* @file testNonlinearConstraints.cpp
* @brief
* @author Duy-Nguyen Ta
* @date Oct 26, 2013
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/LieScalar.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#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<LieScalar, LieScalar,
LieScalar> {
typedef NonlinearConstraint3<LieScalar, LieScalar, LieScalar> 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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> 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<Matrix>& G11, std::vector<Matrix>& G12,
std::vector<Matrix>& G13, std::vector<Matrix>& G22,
std::vector<Matrix>& G23, std::vector<Matrix>& 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<Matrix> expectedG11, expectedG12, expectedG13, expectedG22,
expectedG23, expectedG33;
constraint.expectedHessians(x1, x2, x3, expectedG11, expectedG12, expectedG13,
expectedG22, expectedG23, expectedG33);
std::vector<Matrix> 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<Point2, Point2, Point2> {
typedef NonlinearConstraint3<Point2, Point2, Point2> 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<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none, boost::optional<Matrix&> 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<Matrix>& G11, std::vector<Matrix>& G12,
std::vector<Matrix>& G13, std::vector<Matrix>& G22,
std::vector<Matrix>& G23, std::vector<Matrix>& 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<Matrix> expectedG11, expectedG12, expectedG13, expectedG22,
expectedG23, expectedG33;
constraint.expectedHessians(x1, x2, x3, expectedG11, expectedG12, expectedG13,
expectedG22, expectedG23, expectedG33);
std::vector<Matrix> G11, G12, G13, G22, G23, G33;
constraint.evaluateHessians(x1, x2, x3, G11, G12, G13, G22, G23, G33);
for (size_t i = 0; i<G11.size(); ++i) {
EXPECT(assert_equal(expectedG11[i], G11[i], 1e-5));
EXPECT(assert_equal(expectedG12[i], G12[i], 1e-5));
EXPECT(assert_equal(expectedG13[i], G13[i], 1e-5));
EXPECT(assert_equal(expectedG22[i], G22[i], 1e-5));
EXPECT(assert_equal(expectedG23[i], G23[i], 1e-5));
EXPECT(assert_equal(expectedG33[i], G33[i], 1e-5));
}
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */