Deleted unrealted SQP files.
=
parent
b91e531fac
commit
41684ee6e0
|
|
@ -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 */
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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;
|
||||
|
||||
};
|
||||
}
|
||||
|
|
@ -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 */
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -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;
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -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 */
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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 */
|
||||
|
|
@ -1 +1 @@
|
|||
gtsamAddTestsGlob(nonlinear_unstable "test*.cpp" "testCustomChartExpression.cpp;testLCNLPSolver2.cpp" "gtsam_unstable")
|
||||
gtsamAddTestsGlob(nonlinear_unstable "test*.cpp" "testCustomChartExpression.cpp" "gtsam_unstable")
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
//******************************************************************************
|
||||
|
|
@ -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);
|
||||
}
|
||||
//******************************************************************************
|
||||
|
|
@ -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);
|
||||
}
|
||||
//******************************************************************************
|
||||
|
|
@ -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() {
|
||||
|
||||
}
|
||||
|
|
@ -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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue