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

[REFACTOR] Ran Eclipse Code Formatter on all Added files.

release/4.3a0
= 2016-06-13 22:58:36 -04:00
parent bcb5ca97e0
commit b387a08b66
13 changed files with 210 additions and 189 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
gtsam_unstable/linear/ActiveSetSolver.h
View File

@ -43,8 +43,7 @@ public:
* Dual Jacobians used to build a dual factor graph.
*/
template<typename FACTOR>
TermsContainer collectDualJacobians(Key key,
const FactorGraph<FACTOR>& graph,
TermsContainer collectDualJacobians(Key key, const FactorGraph<FACTOR>& graph,
const VariableIndex& variableIndex) const {
/*
* Iterates through each factor in the factor graph and checks
@ -53,43 +52,44 @@ public:
*/
TermsContainer Aterms;
if (variableIndex.find(key) != variableIndex.end()) {
for(size_t factorIx: variableIndex[key]) {
typename FACTOR::shared_ptr factor = graph.at(factorIx);
if (!factor->active()) continue;
Matrix Ai = factor->getA(factor->find(key)).transpose();
Aterms.push_back(std::make_pair(factor->dualKey(), Ai));
for (size_t factorIx : variableIndex[key]) {
typename FACTOR::shared_ptr factor = graph.at(factorIx);
if (!factor->active())
continue;
Matrix Ai = factor->getA(factor->find(key)).transpose();
Aterms.push_back(std::make_pair(factor->dualKey(), Ai));
}
}
return Aterms;
}
return Aterms;
}
/**
* Identifies active constraints that shouldn't be active anymore.
*/
int identifyLeavingConstraint(const InequalityFactorGraph& workingSet,
const VectorValues& lambdas) const;
/**
* Identifies active constraints that shouldn't be active anymore.
*/
int identifyLeavingConstraint(const InequalityFactorGraph& workingSet,
const VectorValues& lambdas) const;
/**
* Builds a dual graph from the current working set.
*/
GaussianFactorGraph::shared_ptr buildDualGraph(
const InequalityFactorGraph& workingSet, const VectorValues& delta) const;
/**
* Builds a dual graph from the current working set.
*/
GaussianFactorGraph::shared_ptr buildDualGraph(
const InequalityFactorGraph& workingSet, const VectorValues& delta) const;
protected:
/**
* Protected constructor because this class doesn't have any meaning without
* a concrete Programming problem to solve.
*/
ActiveSetSolver() :
constrainedKeys_() {
}
/**
* Protected constructor because this class doesn't have any meaning without
* a concrete Programming problem to solve.
*/
ActiveSetSolver() :
constrainedKeys_() {
}
/**
* Computes the distance to move from the current point being examined to the next
* location to be examined by the graph. This should only be used where there are less
* than two constraints active.
*/
boost::tuple<double, int> computeStepSize(
const InequalityFactorGraph& workingSet, const VectorValues& xk,
const VectorValues& p, const double& startAlpha) const;
/**
* Computes the distance to move from the current point being examined to the next
* location to be examined by the graph. This should only be used where there are less
* than two constraints active.
*/
boost::tuple<double, int> computeStepSize(
const InequalityFactorGraph& workingSet, const VectorValues& xk,
const VectorValues& p, const double& startAlpha) const;
};
} // namespace gtsam

6
gtsam_unstable/linear/EqualityFactorGraph.h
View File

@ -26,7 +26,7 @@ namespace gtsam {
* This class is used to represent an equality constraint on
* a Programming problem of the form Ax = b.
*/
class EqualityFactorGraph : public FactorGraph<LinearEquality> {
class EqualityFactorGraph: public FactorGraph<LinearEquality> {
public:
typedef boost::shared_ptr<EqualityFactorGraph> shared_ptr;
@ -36,8 +36,8 @@ public:
*/
double error(const VectorValues& x) const {
double total_error = 0.;
for(const sharedFactor& factor: *this){
if(factor)
for (const sharedFactor& factor : *this) {
if (factor)
total_error += factor->error(x);
}
return total_error;

8
gtsam_unstable/linear/InequalityFactorGraph.h
View File

@ -47,10 +47,10 @@ public:
* Compute error of a guess.
* Infinity error if it violates an inequality; zero otherwise. */
double error(const VectorValues& x) const {
for(const sharedFactor& factor: *this) {
if(factor)
if (factor->error(x) > 0)
return std::numeric_limits<double>::infinity();
for (const sharedFactor& factor : *this) {
if (factor)
if (factor->error(x) > 0)
return std::numeric_limits<double>::infinity();
}
return 0.0;
}

1
gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h
View File

@ -5,7 +5,6 @@
* @date 1/24/16
*/
namespace gtsam {
class InfeasibleOrUnboundedProblem: public ThreadsafeException<

4
gtsam_unstable/linear/LPInitSolver.h
View File

@ -128,7 +128,7 @@ private:
/// Collect all terms of a factor into a container.
std::vector<std::pair<Key, Matrix> > collectTerms(
const LinearInequality& factor) const {
std::vector<std::pair<Key, Matrix> > terms;
std::vector < std::pair<Key, Matrix> > terms;
for (Factor::const_iterator it = factor.begin(); it != factor.end(); it++) {
terms.push_back(make_pair(*it, factor.getA(it)));
}
@ -140,7 +140,7 @@ private:
const InequalityFactorGraph& inequalities) const {
InequalityFactorGraph slackInequalities;
for (const auto& factor : lp_.inequalities) {
std::vector<std::pair<Key, Matrix> > terms = collectTerms(*factor); // Cx
std::vector < std::pair<Key, Matrix> > terms = collectTerms(*factor); // Cx
terms.push_back(make_pair(yKey, Matrix::Constant(1, 1, -1.0))); // -y
double d = factor->getb()[0];
slackInequalities.push_back(

12
gtsam_unstable/linear/LPSolver.cpp
View File

@ -109,7 +109,7 @@ GaussianFactorGraph::shared_ptr LPSolver::createLeastSquareFactors(
lp_.cost.end(), std::inserter(difference, difference.end()));
for (Key k : difference) {
size_t dim = keysDim_.at(k);
graph->push_back(JacobianFactor(k, Matrix::Identity(dim,dim), xk.at(k)));
graph->push_back(JacobianFactor(k, Matrix::Identity(dim, dim), xk.at(k)));
}
}
return graph;
@ -136,10 +136,10 @@ boost::shared_ptr<JacobianFactor> LPSolver::createDualFactor(Key key,
const InequalityFactorGraph &workingSet, const VectorValues &delta) const {
// Transpose the A matrix of constrained factors to have the jacobian of the
// dual key
TermsContainer Aterms = collectDualJacobians<LinearEquality>(key,
lp_.equalities, equalityVariableIndex_);
TermsContainer AtermsInequalities = collectDualJacobians<LinearInequality>(
key, workingSet, inequalityVariableIndex_);
TermsContainer Aterms = collectDualJacobians < LinearEquality
> (key, lp_.equalities, equalityVariableIndex_);
TermsContainer AtermsInequalities = collectDualJacobians < LinearInequality
> (key, workingSet, inequalityVariableIndex_);
Aterms.insert(Aterms.end(), AtermsInequalities.begin(),
AtermsInequalities.end());
@ -149,7 +149,7 @@ boost::shared_ptr<JacobianFactor> LPSolver::createDualFactor(Key key,
Factor::const_iterator it = lp_.cost.find(key);
if (it != lp_.cost.end())
b = lp_.cost.getA(it).transpose();
return boost::make_shared<JacobianFactor>(Aterms, b); // compute the least-square approximation of dual variables
return boost::make_shared < JacobianFactor > (Aterms, b); // compute the least-square approximation of dual variables
} else {
return boost::make_shared<JacobianFactor>();
}

137
gtsam_unstable/linear/LinearCost.h
View File

@ -30,90 +30,89 @@ typedef Eigen::RowVectorXd RowVector;
*/
class LinearCost: public JacobianFactor {
public:
typedef LinearCost This; ///< Typedef to this class
typedef JacobianFactor Base; ///< Typedef to base class
typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
typedef LinearCost This; ///< Typedef to this class
typedef JacobianFactor Base; ///< Typedef to base class
typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
public:
/** default constructor for I/O */
LinearCost() :
Base() {
}
/** default constructor for I/O */
LinearCost() :
Base() {
}
/** Conversion from HessianFactor */
explicit LinearCost(const HessianFactor& hf) {
throw std::runtime_error("Cannot convert HessianFactor to LinearCost");
}
/** Conversion from HessianFactor */
explicit LinearCost(const HessianFactor& hf) {
throw std::runtime_error("Cannot convert HessianFactor to LinearCost");
}
/** Conversion from JacobianFactor */
explicit LinearCost(const JacobianFactor& jf) :
Base(jf) {
if (jf.isConstrained()) {
throw std::runtime_error(
"Cannot convert a constrained JacobianFactor to LinearCost");
}
/** Conversion from JacobianFactor */
explicit LinearCost(const JacobianFactor& jf) :
Base(jf) {
if (jf.isConstrained()) {
throw std::runtime_error(
"Cannot convert a constrained JacobianFactor to LinearCost");
}
if (jf.get_model()->dim() != 1) {
throw std::runtime_error(
"Only support single-valued linear cost factor!");
}
}
if (jf.get_model()->dim() != 1) {
throw std::runtime_error(
"Only support single-valued linear cost factor!");
}
}
/** Construct unary factor */
LinearCost(Key i1, const RowVector& A1) :
Base(i1, A1, Vector1::Zero()) {
}
/** Construct unary factor */
LinearCost(Key i1, const RowVector& A1) :
Base(i1, A1, Vector1::Zero()) {
}
/** Construct binary factor */
LinearCost(Key i1, const RowVector& A1, Key i2, const RowVector& A2,
double b) :
Base(i1, A1, i2, A2, Vector1::Zero()) {
}
/** Construct binary factor */
LinearCost(Key i1, const RowVector& A1, Key i2, const RowVector& A2, double b) :
Base(i1, A1, i2, A2, Vector1::Zero()) {
}
/** Construct ternary factor */
LinearCost(Key i1, const RowVector& A1, Key i2, const RowVector& A2, Key i3,
const RowVector& A3) :
Base(i1, A1, i2, A2, i3, A3, Vector1::Zero()) {
}
/** Construct ternary factor */
LinearCost(Key i1, const RowVector& A1, Key i2, const RowVector& A2, Key i3,
const RowVector& A3) :
Base(i1, A1, i2, A2, i3, A3, Vector1::Zero()) {
}
/** Construct an n-ary factor
* @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the
* collection of keys and matrices making up the factor. */
template<typename TERMS>
LinearCost(const TERMS& terms) :
Base(terms, Vector1::Zero()) {
}
/** Construct an n-ary factor
* @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the
* collection of keys and matrices making up the factor. */
template<typename TERMS>
LinearCost(const TERMS& terms) :
Base(terms, Vector1::Zero()) {
}
/** Virtual destructor */
virtual ~LinearCost() {
}
/** Virtual destructor */
virtual ~LinearCost() {
}
/** equals */
virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const {
return Base::equals(lf, tol);
}
/** equals */
virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const {
return Base::equals(lf, tol);
}
/** print */
virtual void print(const std::string& s = "",
const KeyFormatter& formatter = DefaultKeyFormatter) const {
Base::print(s + " LinearCost: ", formatter);
}
/** print */
virtual void print(const std::string& s = "", const KeyFormatter& formatter =
DefaultKeyFormatter) const {
Base::print(s + " LinearCost: ", formatter);
}
/** Clone this LinearCost */
virtual GaussianFactor::shared_ptr clone() const {
return boost::static_pointer_cast<GaussianFactor>(
boost::make_shared<LinearCost>(*this));
}
/** Clone this LinearCost */
virtual GaussianFactor::shared_ptr clone() const {
return boost::static_pointer_cast < GaussianFactor
> (boost::make_shared < LinearCost > (*this));
}
/** Special error_vector for constraints (A*x-b) */
Vector error_vector(const VectorValues& c) const {
return unweighted_error(c);
}
/** Special error_vector for constraints (A*x-b) */
Vector error_vector(const VectorValues& c) const {
return unweighted_error(c);
}
/** Special error for single-valued inequality constraints. */
virtual double error(const VectorValues& c) const {
return error_vector(c)[0];
}
/** Special error for single-valued inequality constraints. */
virtual double error(const VectorValues& c) const {
return error_vector(c)[0];
}
};
// \ LinearCost

26
gtsam_unstable/linear/LinearEquality.h
View File

@ -44,13 +44,14 @@ public:
/**
* Construct from a constrained noisemodel JacobianFactor with a dual key.
*/
explicit LinearEquality(const JacobianFactor& jf, Key dualKey) : Base(jf), dualKey_(dualKey){
explicit LinearEquality(const JacobianFactor& jf, Key dualKey) :
Base(jf), dualKey_(dualKey) {
if (!jf.isConstrained()) {
throw std::runtime_error("Cannot convert an unconstrained JacobianFactor to LinearEquality");
throw std::runtime_error(
"Cannot convert an unconstrained JacobianFactor to LinearEquality");
}
}
/** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */
explicit LinearEquality(const HessianFactor& hf) {
throw std::runtime_error("Cannot convert HessianFactor to LinearEquality");
@ -100,15 +101,19 @@ public:
/** Clone this LinearEquality */
virtual GaussianFactor::shared_ptr clone() const {
return boost::static_pointer_cast<GaussianFactor>(
boost::make_shared<LinearEquality>(*this));
return boost::static_pointer_cast < GaussianFactor
> (boost::make_shared < LinearEquality > (*this));
}
/// dual key
Key dualKey() const { return dualKey_; }
Key dualKey() const {
return dualKey_;
}
/// for active set method: equality constraints are always active
bool active() const { return true; }
bool active() const {
return true;
}
/** Special error_vector for constraints (A*x-b) */
Vector error_vector(const VectorValues& c) const {
@ -123,11 +128,12 @@ public:
return 0.0;
}
}; // \ LinearEquality
};
// \ LinearEquality
/// traits
template<> struct traits<LinearEquality> : public Testable<LinearEquality> {};
template<> struct traits<LinearEquality> : public Testable<LinearEquality> {
};
} // \ namespace gtsam

48
gtsam_unstable/linear/LinearInequality.h
View File

@ -52,9 +52,11 @@ public:
}
/** Conversion from JacobianFactor */
explicit LinearInequality(const JacobianFactor& jf, Key dualKey) : Base(jf), dualKey_(dualKey), active_(true) {
explicit LinearInequality(const JacobianFactor& jf, Key dualKey) :
Base(jf), dualKey_(dualKey), active_(true) {
if (!jf.isConstrained()) {
throw std::runtime_error("Cannot convert an unconstrained JacobianFactor to LinearInequality");
throw std::runtime_error(
"Cannot convert an unconstrained JacobianFactor to LinearInequality");
}
if (jf.get_model()->dim() != 1) {
@ -64,20 +66,20 @@ public:
/** Construct unary factor */
LinearInequality(Key i1, const RowVector& A1, double b, Key dualKey) :
Base(i1, A1, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_(
dualKey), active_(true) {
Base(i1, A1, (Vector(1) << b).finished(),
noiseModel::Constrained::All(1)), dualKey_(dualKey), active_(true) {
}
/** Construct binary factor */
LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2, double b,
Key dualKey) :
Base(i1, A1, i2, A2, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_(
dualKey), active_(true) {
LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2,
double b, Key dualKey) :
Base(i1, A1, i2, A2, (Vector(1) << b).finished(),
noiseModel::Constrained::All(1)), dualKey_(dualKey), active_(true) {
}
/** Construct ternary factor */
LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2, Key i3,
const RowVector& A3, double b, Key dualKey) :
LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2,
Key i3, const RowVector& A3, double b, Key dualKey) :
Base(i1, A1, i2, A2, i3, A3, (Vector(1) << b).finished(),
noiseModel::Constrained::All(1)), dualKey_(dualKey), active_(true) {
}
@ -112,21 +114,29 @@ public:
/** Clone this LinearInequality */
virtual GaussianFactor::shared_ptr clone() const {
return boost::static_pointer_cast<GaussianFactor>(
boost::make_shared<LinearInequality>(*this));
return boost::static_pointer_cast < GaussianFactor
> (boost::make_shared < LinearInequality > (*this));
}
/// dual key
Key dualKey() const { return dualKey_; }
Key dualKey() const {
return dualKey_;
}
/// return true if this constraint is active
bool active() const { return active_; }
bool active() const {
return active_;
}
/// Make this inequality constraint active
void activate() { active_ = true; }
void activate() {
active_ = true;
}
/// Make this inequality constraint inactive
void inactivate() { active_ = false; }
void inactivate() {
active_ = false;
}
/** Special error_vector for constraints (A*x-b) */
Vector error_vector(const VectorValues& c) const {
@ -149,10 +159,12 @@ public:
return aTp;
}
}; // \ LinearInequality
};
// \ LinearInequality
/// traits
template<> struct traits<LinearInequality> : public Testable<LinearInequality> {};
template<> struct traits<LinearInequality> : public Testable<LinearInequality> {
};
} // \ namespace gtsam

3
gtsam_unstable/linear/QP.h
View File

@ -42,7 +42,8 @@ struct QP {
QP(const GaussianFactorGraph& _cost,
const EqualityFactorGraph& _linearEqualities,
const InequalityFactorGraph& _linearInequalities) :
cost(_cost), equalities(_linearEqualities), inequalities(_linearInequalities) {
cost(_cost), equalities(_linearEqualities), inequalities(
_linearInequalities) {
}
/** print */

4
gtsam_unstable/linear/RawQP.h
View File

@ -36,9 +36,7 @@ private:
public:
RawQP() :
row_to_constraint_v(), E(), IG(), IL(), varNumber(1),
b(), g(), varname_to_key(), H(), f(),
obj_name(), name_(), up(), lo(), Free() {
row_to_constraint_v(), E(), IG(), IL(), varNumber(1), b(), g(), varname_to_key(), H(), f(), obj_name(), name_(), up(), lo(), Free() {
}
void setName(

53
gtsam_unstable/linear/tests/testLinearEquality.cpp
View File

@ -28,20 +28,20 @@ using namespace std;
using namespace gtsam;
using namespace boost::assign;
GTSAM_CONCEPT_TESTABLE_INST(LinearEquality)
GTSAM_CONCEPT_TESTABLE_INST (LinearEquality)
namespace {
namespace simple {
// Terms we'll use
const vector<pair<Key, Matrix> > terms = list_of<pair<Key,Matrix> >
(make_pair(5, Matrix3::Identity()))
(make_pair(10, 2*Matrix3::Identity()))
(make_pair(15, 3*Matrix3::Identity()));
namespace simple {
// Terms we'll use
const vector<pair<Key, Matrix> > terms = list_of < pair<Key, Matrix>
> (make_pair(5, Matrix3::Identity()))(
make_pair(10, 2 * Matrix3::Identity()))(
make_pair(15, 3 * Matrix3::Identity()));
// RHS and sigmas
const Vector b = (Vector(3) << 1., 2., 3.).finished();
const SharedDiagonal noise = noiseModel::Constrained::All(3);
}
// RHS and sigmas
const Vector b = (Vector(3) << 1., 2., 3.).finished();
const SharedDiagonal noise = noiseModel::Constrained::All(3);
}
}
/* ************************************************************************* */
@ -53,7 +53,7 @@ TEST(LinearEquality, constructors_and_accessors)
{
// One term constructor
LinearEquality expected(
boost::make_iterator_range(terms.begin(), terms.begin() + 1), b, 0);
boost::make_iterator_range(terms.begin(), terms.begin() + 1), b, 0);
LinearEquality actual(terms[0].first, terms[0].second, b, 0);
EXPECT(assert_equal(expected, actual));
LONGS_EQUAL((long)terms[0].first, (long)actual.keys().back());
@ -65,9 +65,9 @@ TEST(LinearEquality, constructors_and_accessors)
{
// Two term constructor
LinearEquality expected(
boost::make_iterator_range(terms.begin(), terms.begin() + 2), b, 0);
boost::make_iterator_range(terms.begin(), terms.begin() + 2), b, 0);
LinearEquality actual(terms[0].first, terms[0].second,
terms[1].first, terms[1].second, b, 0);
terms[1].first, terms[1].second, b, 0);
EXPECT(assert_equal(expected, actual));
LONGS_EQUAL((long)terms[1].first, (long)actual.keys().back());
EXPECT(assert_equal(terms[1].second, actual.getA(actual.end() - 1)));
@ -78,9 +78,9 @@ TEST(LinearEquality, constructors_and_accessors)
{
// Three term constructor
LinearEquality expected(
boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, 0);
boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, 0);
LinearEquality actual(terms[0].first, terms[0].second,
terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, 0);
terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, 0);
EXPECT(assert_equal(expected, actual));
LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back());
EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1)));
@ -93,10 +93,10 @@ TEST(LinearEquality, constructors_and_accessors)
/* ************************************************************************* */
TEST(LinearEquality, Hessian_conversion) {
HessianFactor hessian(0, (Matrix(4,4) <<
1.57, 2.695, -1.1, -2.35,
2.695, 11.3125, -0.65, -10.225,
-1.1, -0.65, 1, 0.5,
-2.35, -10.225, 0.5, 9.25).finished(),
1.57, 2.695, -1.1, -2.35,
2.695, 11.3125, -0.65, -10.225,
-1.1, -0.65, 1, 0.5,
-2.35, -10.225, 0.5, 9.25).finished(),
(Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(),
73.1725);
@ -169,8 +169,8 @@ TEST(LinearEquality, matrices)
augmentedJacobianExpected << jacobianExpected, rhsExpected;
Matrix augmentedHessianExpected =
augmentedJacobianExpected.transpose() * simple::noise->R().transpose()
* simple::noise->R() * augmentedJacobianExpected;
augmentedJacobianExpected.transpose() * simple::noise->R().transpose()
* simple::noise->R() * augmentedJacobianExpected;
// Whitened Jacobian
EXPECT(assert_equal(jacobianExpected, factor.jacobian().first));
@ -210,8 +210,8 @@ TEST(LinearEquality, operators )
// test gradient at zero
Matrix A; Vector b2; boost::tie(A,b2) = lf.jacobian();
VectorValues expectedG;
expectedG.insert(1, (Vector(2) << 0.2, -0.1).finished());
expectedG.insert(2, (Vector(2) << -0.2, 0.1).finished());
expectedG.insert(1, (Vector(2) << 0.2, -0.1).finished());
expectedG.insert(2, (Vector(2) << -0.2, 0.1).finished());
VectorValues actualG = lf.gradientAtZero();
EXPECT(assert_equal(expectedG, actualG));
}
@ -233,5 +233,8 @@ TEST(LinearEquality, empty )
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

29
gtsam_unstable/linear/tests/testQPSolver.cpp
View File

@ -42,8 +42,8 @@ QP createTestCase() {
//TODO: THIS TEST MIGHT BE WRONG : the last parameter might be 5 instead of 10 because the form of the equation
// Should be 0.5x'Gx + gx + f : Nocedal 449
qp.cost.push_back(
HessianFactor(X(1), X(2), 2.0 * I_1x1, -I_1x1,
3.0 * I_1x1, 2.0 * I_1x1, Z_1x1, 10.0));
HessianFactor(X(1), X(2), 2.0 * I_1x1, -I_1x1, 3.0 * I_1x1, 2.0 * I_1x1,
Z_1x1, 10.0));
// Inequality constraints
qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2
@ -96,8 +96,8 @@ QP createEqualityConstrainedTest() {
// 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
// Hence, we have G11=2, G12 = 0, g1 = 0, G22 = 2, g2 = 0, f = 0
qp.cost.push_back(
HessianFactor(X(1), X(2), 2.0 * I_1x1, Z_1x1, Z_1x1,
2.0 * I_1x1, Z_1x1, 0.0));
HessianFactor(X(1), X(2), 2.0 * I_1x1, Z_1x1, Z_1x1, 2.0 * I_1x1, Z_1x1,
0.0));
// Equality constraints
// x1 + x2 = 1 --> x1 + x2 -1 = 0, hence we negate the b vector
@ -140,9 +140,9 @@ TEST(QPSolver, indentifyActiveConstraints) {
qp.inequalities, currentSolution);
CHECK(!workingSet.at(0)->active()); // inactive
CHECK(workingSet.at(1)->active()); // active
CHECK(workingSet.at(2)->active()); // active
CHECK(!workingSet.at(3)->active()); // inactive
CHECK(workingSet.at(1)->active());// active
CHECK(workingSet.at(2)->active());// active
CHECK(!workingSet.at(3)->active());// inactive
VectorValues solution = solver.solveWithCurrentWorkingSet(workingSet);
VectorValues expectedSolution;
@ -211,14 +211,15 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) {
expectedSolution.insert(X(2), (Vector(1) << 0.5).finished());
CHECK(assert_equal(expectedSolution, solution, 1e-100));
}
pair<QP, QP> testParser(QPSParser parser) {
QP exampleqp = parser.Parse();
QP expectedqp;
Key X1(Symbol('X', 1)), X2(Symbol('X', 2));
// min f(x,y) = 4 + 1.5x -y + 0.58x^2 + 2xy + 2yx + 10y^2
expectedqp.cost.push_back(
HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1, 1.5 * kOne,
10.0 * I_1x1, -2.0 * kOne, 4.0));
HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1, 1.5 * kOne, 10.0 * I_1x1,
-2.0 * kOne, 4.0));
// 2x + y >= 2
// -x + 2y <= 6
expectedqp.inequalities.push_back(
@ -267,13 +268,15 @@ QP createTestMatlabQPEx() {
// 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
// Hence, we have G11=1, G12 = -1, g1 = +2, G22 = 2, g2 = +6, f = 0
qp.cost.push_back(
HessianFactor(X(1), X(2), 1.0 * I_1x1, -I_1x1, 2.0 * I_1x1,
2.0 * I_1x1, 6 * I_1x1, 1000.0));
HessianFactor(X(1), X(2), 1.0 * I_1x1, -I_1x1, 2.0 * I_1x1, 2.0 * I_1x1,
6 * I_1x1, 1000.0));
// Inequality constraints
qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2
qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 1)); //-x1 + 2*x2 <=2
qp.inequalities.push_back(LinearInequality(X(1), 2 * I_1x1, X(2), I_1x1, 3, 2)); // 2*x1 + x2 <=3
qp.inequalities.push_back(
LinearInequality(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 1)); //-x1 + 2*x2 <=2
qp.inequalities.push_back(
LinearInequality(X(1), 2 * I_1x1, X(2), I_1x1, 3, 2)); // 2*x1 + x2 <=3
qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0, 3)); // -x1 <= 0
qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 4)); // -x2 <= 0