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

6
gtsam_unstable/linear/ActiveSetSolver.h
View File

@ -43,8 +43,7 @@ public:
* Dual Jacobians used to build a dual factor graph. * Dual Jacobians used to build a dual factor graph.
*/ */
template<typename FACTOR> template<typename FACTOR>
TermsContainer collectDualJacobians(Key key, TermsContainer collectDualJacobians(Key key, const FactorGraph<FACTOR>& graph,
const FactorGraph<FACTOR>& graph,
const VariableIndex& variableIndex) const { const VariableIndex& variableIndex) const {
/* /*
* Iterates through each factor in the factor graph and checks * Iterates through each factor in the factor graph and checks
@ -55,7 +54,8 @@ public:
if (variableIndex.find(key) != variableIndex.end()) { if (variableIndex.find(key) != variableIndex.end()) {
for (size_t factorIx : variableIndex[key]) { for (size_t factorIx : variableIndex[key]) {
typename FACTOR::shared_ptr factor = graph.at(factorIx); typename FACTOR::shared_ptr factor = graph.at(factorIx);
if (!factor->active()) continue; if (!factor->active())
continue;
Matrix Ai = factor->getA(factor->find(key)).transpose(); Matrix Ai = factor->getA(factor->find(key)).transpose();
Aterms.push_back(std::make_pair(factor->dualKey(), Ai)); Aterms.push_back(std::make_pair(factor->dualKey(), Ai));
} }

1
gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h
View File

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

8
gtsam_unstable/linear/LPSolver.cpp
View File

@ -136,10 +136,10 @@ boost::shared_ptr<JacobianFactor> LPSolver::createDualFactor(Key key,
const InequalityFactorGraph &workingSet, const VectorValues &delta) const { const InequalityFactorGraph &workingSet, const VectorValues &delta) const {
// Transpose the A matrix of constrained factors to have the jacobian of the // Transpose the A matrix of constrained factors to have the jacobian of the
// dual key // dual key
TermsContainer Aterms = collectDualJacobians<LinearEquality>(key, TermsContainer Aterms = collectDualJacobians < LinearEquality
lp_.equalities, equalityVariableIndex_); > (key, lp_.equalities, equalityVariableIndex_);
TermsContainer AtermsInequalities = collectDualJacobians<LinearInequality>( TermsContainer AtermsInequalities = collectDualJacobians < LinearInequality
key, workingSet, inequalityVariableIndex_); > (key, workingSet, inequalityVariableIndex_);
Aterms.insert(Aterms.end(), AtermsInequalities.begin(), Aterms.insert(Aterms.end(), AtermsInequalities.begin(),
AtermsInequalities.end()); AtermsInequalities.end());

11
gtsam_unstable/linear/LinearCost.h
View File

@ -65,8 +65,7 @@ public:
} }
/** Construct binary factor */ /** Construct binary factor */
LinearCost(Key i1, const RowVector& A1, Key i2, const RowVector& A2, LinearCost(Key i1, const RowVector& A1, Key i2, const RowVector& A2, double b) :
double b) :
Base(i1, A1, i2, A2, Vector1::Zero()) { Base(i1, A1, i2, A2, Vector1::Zero()) {
} }
@ -94,15 +93,15 @@ public:
} }
/** print */ /** print */
virtual void print(const std::string& s = "", virtual void print(const std::string& s = "", const KeyFormatter& formatter =
const KeyFormatter& formatter = DefaultKeyFormatter) const { DefaultKeyFormatter) const {
Base::print(s + " LinearCost: ", formatter); Base::print(s + " LinearCost: ", formatter);
} }
/** Clone this LinearCost */ /** Clone this LinearCost */
virtual GaussianFactor::shared_ptr clone() const { virtual GaussianFactor::shared_ptr clone() const {
return boost::static_pointer_cast<GaussianFactor>( return boost::static_pointer_cast < GaussianFactor
boost::make_shared<LinearCost>(*this)); > (boost::make_shared < LinearCost > (*this));
} }
/** Special error_vector for constraints (A*x-b) */ /** Special error_vector for constraints (A*x-b) */

26
gtsam_unstable/linear/LinearEquality.h
View File

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

48
gtsam_unstable/linear/LinearInequality.h
View File

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

3
gtsam_unstable/linear/QP.h
View File

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

4
gtsam_unstable/linear/RawQP.h
View File

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

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

@ -33,10 +33,10 @@ GTSAM_CONCEPT_TESTABLE_INST(LinearEquality)
namespace { namespace {
namespace simple { namespace simple {
// Terms we'll use // Terms we'll use
const vector<pair<Key, Matrix> > terms = list_of<pair<Key,Matrix> > const vector<pair<Key, Matrix> > terms = list_of < pair<Key, Matrix>
(make_pair(5, Matrix3::Identity())) > (make_pair(5, Matrix3::Identity()))(
(make_pair(10, 2*Matrix3::Identity())) make_pair(10, 2 * Matrix3::Identity()))(
(make_pair(15, 3*Matrix3::Identity())); make_pair(15, 3 * Matrix3::Identity()));
// RHS and sigmas // RHS and sigmas
const Vector b = (Vector(3) << 1., 2., 3.).finished(); const Vector b = (Vector(3) << 1., 2., 3.).finished();
@ -233,5 +233,8 @@ TEST(LinearEquality, empty )
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);} int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */ /* ************************************************************************* */

23
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 //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 // Should be 0.5x'Gx + gx + f : Nocedal 449
qp.cost.push_back( qp.cost.push_back(
HessianFactor(X(1), X(2), 2.0 * I_1x1, -I_1x1, HessianFactor(X(1), X(2), 2.0 * I_1x1, -I_1x1, 3.0 * I_1x1, 2.0 * I_1x1,
3.0 * I_1x1, 2.0 * I_1x1, Z_1x1, 10.0)); Z_1x1, 10.0));
// Inequality constraints // 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 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 // 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 // Hence, we have G11=2, G12 = 0, g1 = 0, G22 = 2, g2 = 0, f = 0
qp.cost.push_back( qp.cost.push_back(
HessianFactor(X(1), X(2), 2.0 * I_1x1, Z_1x1, Z_1x1, HessianFactor(X(1), X(2), 2.0 * I_1x1, Z_1x1, Z_1x1, 2.0 * I_1x1, Z_1x1,
2.0 * I_1x1, Z_1x1, 0.0)); 0.0));
// Equality constraints // Equality constraints
// x1 + x2 = 1 --> x1 + x2 -1 = 0, hence we negate the b vector // x1 + x2 = 1 --> x1 + x2 -1 = 0, hence we negate the b vector
@ -211,14 +211,15 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) {
expectedSolution.insert(X(2), (Vector(1) << 0.5).finished()); expectedSolution.insert(X(2), (Vector(1) << 0.5).finished());
CHECK(assert_equal(expectedSolution, solution, 1e-100)); CHECK(assert_equal(expectedSolution, solution, 1e-100));
} }
pair<QP, QP> testParser(QPSParser parser) { pair<QP, QP> testParser(QPSParser parser) {
QP exampleqp = parser.Parse(); QP exampleqp = parser.Parse();
QP expectedqp; QP expectedqp;
Key X1(Symbol('X', 1)), X2(Symbol('X', 2)); Key X1(Symbol('X', 1)), X2(Symbol('X', 2));
// min f(x,y) = 4 + 1.5x -y + 0.58x^2 + 2xy + 2yx + 10y^2 // min f(x,y) = 4 + 1.5x -y + 0.58x^2 + 2xy + 2yx + 10y^2
expectedqp.cost.push_back( expectedqp.cost.push_back(
HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1, 1.5 * kOne, HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1, 1.5 * kOne, 10.0 * I_1x1,
10.0 * I_1x1, -2.0 * kOne, 4.0)); -2.0 * kOne, 4.0));
// 2x + y >= 2 // 2x + y >= 2
// -x + 2y <= 6 // -x + 2y <= 6
expectedqp.inequalities.push_back( 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 // 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 // Hence, we have G11=1, G12 = -1, g1 = +2, G22 = 2, g2 = +6, f = 0
qp.cost.push_back( qp.cost.push_back(
HessianFactor(X(1), X(2), 1.0 * I_1x1, -I_1x1, 2.0 * I_1x1, HessianFactor(X(1), X(2), 1.0 * I_1x1, -I_1x1, 2.0 * I_1x1, 2.0 * I_1x1,
2.0 * I_1x1, 6 * I_1x1, 1000.0)); 6 * I_1x1, 1000.0));
// Inequality constraints // 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), 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(
qp.inequalities.push_back(LinearInequality(X(1), 2 * I_1x1, X(2), I_1x1, 3, 2)); // 2*x1 + x2 <=3 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(1), -I_1x1, 0, 3)); // -x1 <= 0
qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 4)); // -x2 <= 0 qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 4)); // -x2 <= 0