disabled test
krunalchande
thduynguyen
parent
2476bbe8d7
commit
3142f0a9a7
|
|
@ -48,18 +48,21 @@
|
|||
// CHECK(!le1.equals(le2));
|
||||
//}
|
||||
//
|
||||
//////******************************************************************************
|
||||
////TEST(testLinearEqualityManifolds, linearize ) {
|
||||
////}
|
||||
////
|
||||
//////******************************************************************************
|
||||
////TEST(testLinearEqualityManifolds, size ) {
|
||||
////}
|
||||
////******************************************************************************
|
||||
//TEST(testLinearEqualityManifolds, linearize ) {
|
||||
//}
|
||||
//
|
||||
////******************************************************************************
|
||||
//TEST(testLinearEqualityManifolds, size ) {
|
||||
//}
|
||||
//
|
||||
////******************************************************************************
|
||||
//int main() {
|
||||
// TestResult tr;
|
||||
// return TestRegistry::runAllTests(tr);
|
||||
//}
|
||||
////******************************************************************************
|
||||
//
|
||||
//******************************************************************************
|
||||
|
||||
int main() {
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -26,112 +26,113 @@
|
|||
#include <iostream>
|
||||
|
||||
|
||||
namespace gtsam {
|
||||
struct LinearlyConstrainedNLP {
|
||||
NonlinearFactorGraph cost;
|
||||
LinearEqualityFactorGraph equalities;
|
||||
LinearInequalityFactorGraph inequalities;
|
||||
};
|
||||
|
||||
struct LinearlyConstrainedNLPState {
|
||||
Values values;
|
||||
VectorValues duals;
|
||||
bool converged;
|
||||
LinearlyConstrainedNLPState(const Values& initialValues) :
|
||||
values(initialValues), duals(VectorValues()), converged(false) {
|
||||
}
|
||||
};
|
||||
class LinearlyConstrainedNonLinearOptimizer {
|
||||
LinearlyConstrainedNLP lcNLP_;
|
||||
public:
|
||||
LinearlyConstrainedNonLinearOptimizer(const LinearlyConstrainedNLP& lcNLP): lcNLP_(lcNLP) {}
|
||||
|
||||
LinearlyConstrainedNLPState iterate(const LinearlyConstrainedNLPState& state) const {
|
||||
QP qp;
|
||||
qp.cost = lcNLP_.cost.linearize(state.values);
|
||||
qp.equalities = lcNLP_.equalities;
|
||||
qp.inequalities = lcNLP_.inequalities;
|
||||
QPSolver qpSolver(qp);
|
||||
VectorValues delta, duals;
|
||||
boost::tie(delta, duals) = qpSolver.optimize();
|
||||
LinearlyConstrainedNLPState newState;
|
||||
newState.values = state.values.retract(delta);
|
||||
newState.duals = duals;
|
||||
newState.converged = checkConvergence(newState.values, newState.duals);
|
||||
return newState;
|
||||
}
|
||||
|
||||
/**
|
||||
* Main optimization function.
|
||||
*/
|
||||
std::pair<Values, VectorValues> optimize(const Values& initialValues) const {
|
||||
LinearlyConstrainedNLPState state(initialValues);
|
||||
while(!state.converged){
|
||||
state = iterate(state);
|
||||
}
|
||||
|
||||
return std::make_pair(initialValues, VectorValues());
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam::symbol_shorthand;
|
||||
using namespace gtsam;
|
||||
const double tol = 1e-10;
|
||||
//******************************************************************************
|
||||
TEST(LinearlyConstrainedNonlinearOptimizer, Problem1 ) {
|
||||
|
||||
// build a quadratic Objective function x1^2 - x1*x2 + x2^2 - 3*x1 + 5
|
||||
// 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, 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),
|
||||
2.0 * ones(1, 1), zero(1), 10.0);
|
||||
|
||||
// build linear 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), 0, 1)); // -x1 <= 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
|
||||
|
||||
// Instantiate LinearlyConstrainedNLP, pretending that the cost is not quadratic
|
||||
// (LinearContainerFactor makes a linear factor behave like a nonlinear one)
|
||||
LinearlyConstrainedNLP lcNLP;
|
||||
lcNLP.cost.add(LinearContainerFactor(lf));
|
||||
lcNLP.inequalities = inequalities;
|
||||
|
||||
// Compare against a QP
|
||||
QP qp;
|
||||
qp.cost.add(lf);
|
||||
qp.inequalities = inequalities;
|
||||
|
||||
// instantiate QPsolver
|
||||
QPSolver qpSolver(qp);
|
||||
// create initial values for optimization
|
||||
VectorValues initialVectorValues;
|
||||
initialVectorValues.insert(X(1), zero(1));
|
||||
initialVectorValues.insert(X(2), zero(1));
|
||||
VectorValues expectedSolution = qpSolver.optimize(initialVectorValues).first;
|
||||
|
||||
// instantiate LinearlyConstrainedNonLinearOptimizer
|
||||
LinearlyConstrainedNonLinearOptimizer lcNLPSolver(lcNLP);
|
||||
// create initial values for optimization
|
||||
Values initialValues;
|
||||
initialValues.insert(X(1), 0.0);
|
||||
initialValues.insert(X(2), 0.0);
|
||||
Values actualSolution = lcNLPSolver.optimize(initialValues).first;
|
||||
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
//namespace gtsam {
|
||||
//struct LinearlyConstrainedNLP {
|
||||
// NonlinearFactorGraph cost;
|
||||
// LinearEqualityFactorGraph equalities;
|
||||
// LinearInequalityFactorGraph inequalities;
|
||||
//};
|
||||
//
|
||||
//struct LinearlyConstrainedNLPState {
|
||||
// Values values;
|
||||
// VectorValues duals;
|
||||
// bool converged;
|
||||
// LinearlyConstrainedNLPState(const Values& initialValues) :
|
||||
// values(initialValues), duals(VectorValues()), converged(false) {
|
||||
// }
|
||||
//};
|
||||
//class LinearlyConstrainedNonLinearOptimizer {
|
||||
// LinearlyConstrainedNLP lcNLP_;
|
||||
//public:
|
||||
// LinearlyConstrainedNonLinearOptimizer(const LinearlyConstrainedNLP& lcNLP): lcNLP_(lcNLP) {}
|
||||
//
|
||||
// LinearlyConstrainedNLPState iterate(const LinearlyConstrainedNLPState& state) const {
|
||||
// QP qp;
|
||||
// qp.cost = lcNLP_.cost.linearize(state.values);
|
||||
// qp.equalities = lcNLP_.equalities;
|
||||
// qp.inequalities = lcNLP_.inequalities;
|
||||
// QPSolver qpSolver(qp);
|
||||
// VectorValues delta, duals;
|
||||
// boost::tie(delta, duals) = qpSolver.optimize();
|
||||
// LinearlyConstrainedNLPState newState;
|
||||
// newState.values = state.values.retract(delta);
|
||||
// newState.duals = duals;
|
||||
// newState.converged = checkConvergence(newState.values, newState.duals);
|
||||
// return newState;
|
||||
// }
|
||||
//
|
||||
// /**
|
||||
// * Main optimization function.
|
||||
// */
|
||||
// std::pair<Values, VectorValues> optimize(const Values& initialValues) const {
|
||||
// LinearlyConstrainedNLPState state(initialValues);
|
||||
// while(!state.converged){
|
||||
// state = iterate(state);
|
||||
// }
|
||||
//
|
||||
// return std::make_pair(initialValues, VectorValues());
|
||||
// }
|
||||
//};
|
||||
//}
|
||||
//
|
||||
//using namespace std;
|
||||
//using namespace gtsam::symbol_shorthand;
|
||||
//using namespace gtsam;
|
||||
//const double tol = 1e-10;
|
||||
////******************************************************************************
|
||||
//TEST(LinearlyConstrainedNonlinearOptimizer, Problem1 ) {
|
||||
//
|
||||
// // build a quadratic Objective function x1^2 - x1*x2 + x2^2 - 3*x1 + 5
|
||||
// // 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, 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),
|
||||
// 2.0 * ones(1, 1), zero(1), 10.0);
|
||||
//
|
||||
// // build linear 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), 0, 1)); // -x1 <= 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
|
||||
//
|
||||
// // Instantiate LinearlyConstrainedNLP, pretending that the cost is not quadratic
|
||||
// // (LinearContainerFactor makes a linear factor behave like a nonlinear one)
|
||||
// LinearlyConstrainedNLP lcNLP;
|
||||
// lcNLP.cost.add(LinearContainerFactor(lf));
|
||||
// lcNLP.inequalities = inequalities;
|
||||
//
|
||||
// // Compare against a QP
|
||||
// QP qp;
|
||||
// qp.cost.add(lf);
|
||||
// qp.inequalities = inequalities;
|
||||
//
|
||||
// // instantiate QPsolver
|
||||
// QPSolver qpSolver(qp);
|
||||
// // create initial values for optimization
|
||||
// VectorValues initialVectorValues;
|
||||
// initialVectorValues.insert(X(1), zero(1));
|
||||
// initialVectorValues.insert(X(2), zero(1));
|
||||
// VectorValues expectedSolution = qpSolver.optimize(initialVectorValues).first;
|
||||
//
|
||||
// // instantiate LinearlyConstrainedNonLinearOptimizer
|
||||
// LinearlyConstrainedNonLinearOptimizer lcNLPSolver(lcNLP);
|
||||
// // create initial values for optimization
|
||||
// Values initialValues;
|
||||
// initialValues.insert(X(1), 0.0);
|
||||
// initialValues.insert(X(2), 0.0);
|
||||
// Values actualSolution = lcNLPSolver.optimize(initialValues).first;
|
||||
//
|
||||
//
|
||||
// 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);
|
||||
//}
|
||||
//
|
||||
//******************************************************************************
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
std::cout<<"here"<<std::endl;
|
||||
// TestResult tr;
|
||||
// return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
//******************************************************************************
|
||||
|
||||
//
|
||||
|
|
|
|||
Loading…
Reference in New Issue