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

Added test to test iterative framework. DOES NOT WORK YET.

release/4.3a0
krunalchande 2015-01-20 22:22:29 -05:00 committed by thduynguyen
parent f1703db4aa
commit 23dbaad23e
1 changed files with 127 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -275,6 +275,25 @@ TEST_DISABLED(testlcnlpSolver, poseWithABoundary) {
CHECK(assert_equal(expectedSolution, actualSolution, 1e-10));
}
TEST_DISABLED(testlcnlpSolver, poseWithNoConstraints) {
//Instantiate LCNLP
LCNLP lcnlp;
lcnlp.cost.add(PriorFactor<Pose3>(X(1), Pose3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 0, 0)), noiseModel::Unit::Create(6)));
Values initialValues;
initialValues.insert(X(1), Pose3(Rot3::ypr(0.3, 0.2, 0.3), Point3(1, 0, 0)));
Values expectedSolution;
expectedSolution.insert(X(1), Pose3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 0, 0)));
// Instantiate LCNLPSolver
LCNLPSolver lcnlpSolver(lcnlp);
Values actualSolution = lcnlpSolver.optimize(initialValues, true, false).first; // TODO: Fails without warmstart
CHECK(assert_equal(expectedSolution, actualSolution, 1e-10));
}
TEST_DISABLED(testlcnlpSolver, poseWithinA2DBox) {
const Key dualKey = 0;
@ -299,7 +318,7 @@ TEST_DISABLED(testlcnlpSolver, poseWithinA2DBox) {
CHECK(assert_equal(expectedSolution, actualSolution, 1e-10));
}
TEST(testlcnlpSolver, posesInA2DBox) {
TEST_DISABLED(testlcnlpSolver, posesInA2DBox) {
const double xLowerBound = -3.0,
xUpperBound = 5.0,
yLowerBound = -1.0,
@ -366,6 +385,113 @@ TEST(testlcnlpSolver, posesInA2DBox) {
CHECK(assert_equal(expectedSolution, actualSolution, 1e-5));
}
TEST_DISABLED(testlcnlpSolver, iterativePoseinBox) {
const double xLowerBound = -1.0,
xUpperBound = 1.0,
yLowerBound = -1.0,
yUpperBound = 1.0,
zLowerBound = -1.0,
zUpperBound = 1.0;
//Instantiate LCNLP
LCNLP lcnlp;
// prior on the first pose
SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(
(Vector(6) << 0.001, 0.001, 0.001, 0.0001, 0.0001, 0.0001).finished());
lcnlp.cost.add(PriorFactor<Pose3>(X(0), Pose3(), priorNoise));
// odometry between factor for subsequent poses
SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas(
(Vector(6) << 0.001, 0.001, 0.001, 0.1, 0.1, 0.1).finished());
Pose3 odo(Rot3::ypr(0, 0, 0), Point3(0.4, 0, 0));
Values initialValues;
Values isamValues;
initialValues.insert(X(0), Pose3(Rot3(), Point3(0, 0, 0)));
isamValues.insert(X(0), Pose3(Rot3(), Point3(0, 0, 0)));
std::pair<Values, VectorValues> solutionAndDuals;
solutionAndDuals.first.insert(X(0), Pose3(Rot3(), Point3(0, 0, 0)));
Values actualSolution;
bool useWarmStart = true;
bool firstTime = true;
// Box constraints
Key dualKey = 0;
for (size_t i=1; i<=4; ++i) {
lcnlp.cost.add(BetweenFactor<Pose3>(X(i-1), X(i), odo, odoNoise));
lcnlp.linearInequalities.add(AxisLowerBound(X(i), X_AXIS, xLowerBound, dualKey++));
lcnlp.linearInequalities.add(AxisUpperBound(X(i), X_AXIS, xUpperBound, dualKey++));
lcnlp.linearInequalities.add(AxisLowerBound(X(i), Y_AXIS, yLowerBound, dualKey++));
lcnlp.linearInequalities.add(AxisUpperBound(X(i), Y_AXIS, yUpperBound, dualKey++));
lcnlp.linearInequalities.add(AxisLowerBound(X(i), Z_AXIS, zLowerBound, dualKey++));
lcnlp.linearInequalities.add(AxisUpperBound(X(i), Z_AXIS, zUpperBound, dualKey++));
initialValues.insert(X(i), Pose3(Rot3(), Point3(0, 0, 0)));
isamValues = solutionAndDuals.first;
isamValues.insert(X(i), solutionAndDuals.first.at(X(i-1)));
isamValues.print("iSAM values: \n");
// Instantiate LCNLPSolver
LCNLPSolver lcnlpSolver(lcnlp);
if (firstTime) {
solutionAndDuals = lcnlpSolver.optimize(isamValues, useWarmStart);
firstTime = false;
}
else {
cout << " using this \n ";
solutionAndDuals = lcnlpSolver.optimize(isamValues, solutionAndDuals.second, useWarmStart);
}
cout << " ************************** \n";
}
actualSolution = solutionAndDuals.first;
cout << "out of loop" << endl;
Values expectedSolution;
expectedSolution.insert(X(0), Pose3());
expectedSolution.insert(X(1), Pose3(Rot3::ypr(0, 0, 0), Point3(0.25, 0, 0)));
expectedSolution.insert(X(2), Pose3(Rot3::ypr(0, 0, 0), Point3(0.50, 0, 0)));
expectedSolution.insert(X(3), Pose3(Rot3::ypr(0, 0, 0), Point3(0.75, 0, 0)));
expectedSolution.insert(X(4), Pose3(Rot3::ypr(0, 0, 0), Point3(1, 0, 0)));
// cout << "Rotation angles: " << endl;
// for (size_t i = 1; i<=3; i++) {
// cout << actualSolution.at<Pose3>(X(i)).rotation().ypr().transpose()*180/M_PI << endl;
// }
cout << "Actual Error: " << lcnlp.cost.error(actualSolution) << endl;
cout << "Expected Error: " << lcnlp.cost.error(expectedSolution) << endl;
actualSolution.print("actualSolution: ");
CHECK(assert_equal(expectedSolution, actualSolution, 1e-5));
}
double testHessian(const Pose3& X) {
return X.transform_from(Point3(0,0,0)).x();
}
Pose3 pose(Rot3::ypr(0.1, 0.4, 0.7), Point3(1, 9.5, 7.3));
TEST_DISABLED(testlcnlpSolver, testingHessian) {
Matrix H = numericalHessian(testHessian, pose, 1e-5);
}
double h3(const Vector6& v) {
return pose.retract(v).translation().x();
}
TEST_DISABLED(testlcnlpSolver, NumericalHessian3) {
Matrix6 expected;
expected.setZero();
Vector6 z;
z.setZero();
EXPECT(assert_equal(expected, numericalHessian(h3, z, 1e-5), 1e-5));
}
//******************************************************************************
int main() {
cout<<"here: "<<endl;