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

gtsam_unstable/nonlinear/tests/testLCNLPSolver.cpp

@@ -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;