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

disabled test

release/4.3a0
krunalchande 2014-12-25 01:30:02 -05:00 committed by thduynguyen
parent 2476bbe8d7
commit 3142f0a9a7
2 changed files with 118 additions and 114 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -48,18 +48,21 @@
// CHECK(!le1.equals(le2)); // CHECK(!le1.equals(le2));
//} //}
// //
//////****************************************************************************** ////******************************************************************************
////TEST(testLinearEqualityManifolds, linearize ) { //TEST(testLinearEqualityManifolds, linearize ) {
////} //}
//// //
//////****************************************************************************** ////******************************************************************************
////TEST(testLinearEqualityManifolds, size ) { //TEST(testLinearEqualityManifolds, size ) {
////} //}
// //
////****************************************************************************** ////******************************************************************************
//int main() { //int main() {
// TestResult tr; // TestResult tr;
// return TestRegistry::runAllTests(tr); // return TestRegistry::runAllTests(tr);
//} //}
////****************************************************************************** //******************************************************************************
//
int main() {
}

211
gtsam_unstable/nonlinear/tests/testLinearlyConstrainedNonlinearOptimizer.cpp
View File

@ -26,112 +26,113 @@
#include <iostream> #include <iostream>
namespace gtsam { //namespace gtsam {
struct LinearlyConstrainedNLP { //struct LinearlyConstrainedNLP {
NonlinearFactorGraph cost; // NonlinearFactorGraph cost;
LinearEqualityFactorGraph equalities; // LinearEqualityFactorGraph equalities;
LinearInequalityFactorGraph inequalities; // LinearInequalityFactorGraph inequalities;
}; //};
//
struct LinearlyConstrainedNLPState { //struct LinearlyConstrainedNLPState {
Values values; // Values values;
VectorValues duals; // VectorValues duals;
bool converged; // bool converged;
LinearlyConstrainedNLPState(const Values& initialValues) : // LinearlyConstrainedNLPState(const Values& initialValues) :
values(initialValues), duals(VectorValues()), converged(false) { // values(initialValues), duals(VectorValues()), converged(false) {
} // }
}; //};
class LinearlyConstrainedNonLinearOptimizer { //class LinearlyConstrainedNonLinearOptimizer {
LinearlyConstrainedNLP lcNLP_; // LinearlyConstrainedNLP lcNLP_;
public: //public:
LinearlyConstrainedNonLinearOptimizer(const LinearlyConstrainedNLP& lcNLP): lcNLP_(lcNLP) {} // LinearlyConstrainedNonLinearOptimizer(const LinearlyConstrainedNLP& lcNLP): lcNLP_(lcNLP) {}
//
LinearlyConstrainedNLPState iterate(const LinearlyConstrainedNLPState& state) const { // LinearlyConstrainedNLPState iterate(const LinearlyConstrainedNLPState& state) const {
QP qp; // QP qp;
qp.cost = lcNLP_.cost.linearize(state.values); // qp.cost = lcNLP_.cost.linearize(state.values);
qp.equalities = lcNLP_.equalities; // qp.equalities = lcNLP_.equalities;
qp.inequalities = lcNLP_.inequalities; // qp.inequalities = lcNLP_.inequalities;
QPSolver qpSolver(qp); // QPSolver qpSolver(qp);
VectorValues delta, duals; // VectorValues delta, duals;
boost::tie(delta, duals) = qpSolver.optimize(); // boost::tie(delta, duals) = qpSolver.optimize();
LinearlyConstrainedNLPState newState; // LinearlyConstrainedNLPState newState;
newState.values = state.values.retract(delta); // newState.values = state.values.retract(delta);
newState.duals = duals; // newState.duals = duals;
newState.converged = checkConvergence(newState.values, newState.duals); // newState.converged = checkConvergence(newState.values, newState.duals);
return newState; // return newState;
} // }
//
/** // /**
* Main optimization function. // * Main optimization function.
*/ // */
std::pair<Values, VectorValues> optimize(const Values& initialValues) const { // std::pair<Values, VectorValues> optimize(const Values& initialValues) const {
LinearlyConstrainedNLPState state(initialValues); // LinearlyConstrainedNLPState state(initialValues);
while(!state.converged){ // while(!state.converged){
state = iterate(state); // state = iterate(state);
} // }
//
return std::make_pair(initialValues, VectorValues()); // return std::make_pair(initialValues, VectorValues());
} // }
}; //};
} //}
//
using namespace std; //using namespace std;
using namespace gtsam::symbol_shorthand; //using namespace gtsam::symbol_shorthand;
using namespace gtsam; //using namespace gtsam;
const double tol = 1e-10; //const double tol = 1e-10;
//****************************************************************************** ////******************************************************************************
TEST(LinearlyConstrainedNonlinearOptimizer, Problem1 ) { //TEST(LinearlyConstrainedNonlinearOptimizer, Problem1 ) {
//
// build a quadratic Objective function x1^2 - x1*x2 + x2^2 - 3*x1 + 5 // // build a quadratic Objective function x1^2 - x1*x2 + x2^2 - 3*x1 + 5
// Note the Hessian encodes: // // Note the Hessian encodes:
// 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 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10 // // Hence, we have G11=2, G12 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10
HessianFactor lf(X(1), X(2), 2.0 * ones(1, 1), -ones(1, 1), 3.0 * ones(1), // HessianFactor lf(X(1), X(2), 2.0 * ones(1, 1), -ones(1, 1), 3.0 * ones(1),
2.0 * ones(1, 1), zero(1), 10.0); // 2.0 * ones(1, 1), zero(1), 10.0);
//
// build linear inequalities // // build linear inequalities
LinearInequalityFactorGraph inequalities; // LinearInequalityFactorGraph inequalities;
inequalities.push_back(LinearInequality(X(1), ones(1,1), X(2), ones(1,1), 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 // inequalities.push_back(LinearInequality(X(1), ones(1,1), X(2), ones(1,1), 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2
inequalities.push_back(LinearInequality(X(1), -ones(1,1), 0, 1)); // -x1 <= 0 // inequalities.push_back(LinearInequality(X(1), -ones(1,1), 0, 1)); // -x1 <= 0
inequalities.push_back(LinearInequality(X(2), -ones(1,1), 0, 2)); // -x2 <= 0 // inequalities.push_back(LinearInequality(X(2), -ones(1,1), 0, 2)); // -x2 <= 0
inequalities.push_back(LinearInequality(X(1), ones(1,1), 1.5, 3)); // x1 <= 3/2 // inequalities.push_back(LinearInequality(X(1), ones(1,1), 1.5, 3)); // x1 <= 3/2
//
// Instantiate LinearlyConstrainedNLP, pretending that the cost is not quadratic // // Instantiate LinearlyConstrainedNLP, pretending that the cost is not quadratic
// (LinearContainerFactor makes a linear factor behave like a nonlinear one) // // (LinearContainerFactor makes a linear factor behave like a nonlinear one)
LinearlyConstrainedNLP lcNLP; // LinearlyConstrainedNLP lcNLP;
lcNLP.cost.add(LinearContainerFactor(lf)); // lcNLP.cost.add(LinearContainerFactor(lf));
lcNLP.inequalities = inequalities; // lcNLP.inequalities = inequalities;
//
// Compare against a QP // // Compare against a QP
QP qp; // QP qp;
qp.cost.add(lf); // qp.cost.add(lf);
qp.inequalities = inequalities; // qp.inequalities = inequalities;
//
// instantiate QPsolver // // instantiate QPsolver
QPSolver qpSolver(qp); // QPSolver qpSolver(qp);
// create initial values for optimization // // create initial values for optimization
VectorValues initialVectorValues; // VectorValues initialVectorValues;
initialVectorValues.insert(X(1), zero(1)); // initialVectorValues.insert(X(1), zero(1));
initialVectorValues.insert(X(2), zero(1)); // initialVectorValues.insert(X(2), zero(1));
VectorValues expectedSolution = qpSolver.optimize(initialVectorValues).first; // VectorValues expectedSolution = qpSolver.optimize(initialVectorValues).first;
//
// instantiate LinearlyConstrainedNonLinearOptimizer // // instantiate LinearlyConstrainedNonLinearOptimizer
LinearlyConstrainedNonLinearOptimizer lcNLPSolver(lcNLP); // LinearlyConstrainedNonLinearOptimizer lcNLPSolver(lcNLP);
// create initial values for optimization // // create initial values for optimization
Values initialValues; // Values initialValues;
initialValues.insert(X(1), 0.0); // initialValues.insert(X(1), 0.0);
initialValues.insert(X(2), 0.0); // initialValues.insert(X(2), 0.0);
Values actualSolution = lcNLPSolver.optimize(initialValues).first; // Values actualSolution = lcNLPSolver.optimize(initialValues).first;
//
//
DOUBLES_EQUAL(expectedSolution.at(X(1))[0], actualSolution.at<double>(X(1)), tol); // DOUBLES_EQUAL(expectedSolution.at(X(1))[0], actualSolution.at<double>(X(1)), tol);
DOUBLES_EQUAL(expectedSolution.at(X(2))[0], actualSolution.at<double>(X(2)), tol); // DOUBLES_EQUAL(expectedSolution.at(X(2))[0], actualSolution.at<double>(X(2)), tol);
} //}
//
//****************************************************************************** //******************************************************************************
int main() { int main() {
TestResult tr; std::cout<<"here"<<std::endl;
return TestRegistry::runAllTests(tr); // TestResult tr;
// return TestRegistry::runAllTests(tr);
} }
//****************************************************************************** //******************************************************************************
//