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

added debug print statements

release/4.3a0
krunalchande 2014-12-23 18:15:54 -05:00 committed by thduynguyen
parent 2791f2930b
commit f4a4ce4325
2 changed files with 18 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam_unstable/nonlinear/LCNLPSolver.h
View File

@ -50,6 +50,16 @@ struct LCNLPState {
LCNLPState(const Values& initialValues) : LCNLPState(const Values& initialValues) :
values(initialValues), duals(VectorValues()), converged(false), iterations(0) { 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;
}
}; };
/** /**

13
gtsam_unstable/nonlinear/tests/testLCNLPSolver.cpp
View File

@ -54,7 +54,7 @@ public:
} }
}; };
TEST(testlcnlpSolver, QPProblem) { TEST_DISABLED(testlcnlpSolver, QPProblem) {
const Key dualKey = 0; const Key dualKey = 0;
// Simple quadratic cost: x1^2 + x2^2 // Simple quadratic cost: x1^2 + x2^2
@ -120,7 +120,7 @@ public:
} }
}; };
TEST(testlcnlpSolver, poseOnALine) { TEST_DISABLED(testlcnlpSolver, poseOnALine) {
const Key dualKey = 0; const Key dualKey = 0;
@ -161,7 +161,7 @@ public:
} }
}; };
TEST(testlcnlpSolver, inequalityConstraint) { TEST_DISABLED(testlcnlpSolver, inequalityConstraint) {
const Key dualKey = 0; const Key dualKey = 0;
// Simple quadratic cost: x^2 + y^2 // Simple quadratic cost: x^2 + y^2
@ -253,7 +253,7 @@ public:
} }
}; };
TEST(testlcnlpSolver, poseWithABoundary) { TEST_DISABLED(testlcnlpSolver, poseWithABoundary) {
const Key dualKey = 0; const Key dualKey = 0;
//Instantiate LCNLP //Instantiate LCNLP
@ -275,7 +275,7 @@ TEST(testlcnlpSolver, poseWithABoundary) {
CHECK(assert_equal(expectedSolution, actualSolution, 1e-10)); CHECK(assert_equal(expectedSolution, actualSolution, 1e-10));
} }
TEST(testlcnlpSolver, poseWithinA2DBox) { TEST_DISABLED(testlcnlpSolver, poseWithinA2DBox) {
const Key dualKey = 0; const Key dualKey = 0;
//Instantiate LCNLP //Instantiate LCNLP
@ -359,6 +359,9 @@ TEST(testlcnlpSolver, posesInA2DBox) {
// cout << "Expected Error: " << lcnlp.cost.error(expectedSolution) << endl; // cout << "Expected Error: " << lcnlp.cost.error(expectedSolution) << endl;
// actualSolution.print("actualSolution: "); // 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)); CHECK(assert_equal(expectedSolution, actualSolution, 1e-5));
} }