Nonlinear multifrontal solver

gtsam/base/tests/testSymmetricBlockMatrix.cpp

@@ -155,6 +155,32 @@ TEST(SymmetricBlockMatrix, expressions)
   EXPECT(assert_equal(Matrix(expected2.selfadjointView()), bm6.selfadjointView()));
 }
 
+/* ************************************************************************* */
+// Verify diagonal-only update helpers.
+TEST(SymmetricBlockMatrix, AddDiagonal) {
+  const std::vector<size_t> dimensions{2, 1};
+  SymmetricBlockMatrix bm(dimensions);
+  bm.setZero();
+
+  bm.addScaledIdentity(0, 2.0);
+  bm.addScaledIdentity(1, 3.0);
+
+  Vector delta0(2);
+  delta0 << 1.0, 4.0;
+  bm.addToDiagonalBlock(0, delta0);
+
+  Vector delta1(1);
+  delta1 << -1.0;
+  bm.addToDiagonalBlock(1, delta1);
+
+  Matrix expected = Matrix::Zero(3, 3);
+  expected(0, 0) = 3.0;
+  expected(1, 1) = 6.0;
+  expected(2, 2) = 2.0;
+
+  EXPECT(assert_equal(expected, Matrix(bm.selfadjointView())));
+}
+
 /* ************************************************************************* */
 // Update via block mapping.
 TEST(SymmetricBlockMatrix, UpdateFromMappedBlocks)

gtsam/constrained/AugmentedLagrangianOptimizer.cpp

@@ -29,7 +29,7 @@ namespace gtsam {
  * This factor is used in augmented Lagrangian optimizer to create biased cost
  * functions.
  * Note that the noise model is stored twice (both in original factor and the
- * noisemodel of substitute factor. The noisemodel in the original factor will
+ * noise model of substitute factor. The noise model in the original factor will
  * be ignored. */
 class GTSAM_EXPORT BiasedFactor : public NoiseModelFactor {
  protected:
@@ -117,7 +117,7 @@ AugmentedLagrangianOptimizer::iterate(const State& state, const double muEq,
   auto optimizer =
       createUnconstrainedOptimizer(augmentedLagrangian, state.values);
   newState.setValues(optimizer->optimize(), problem_);
-  newState.unconstrainedIterationss = optimizer->iterations();
+  newState.unconstrainedIterations = optimizer->iterations();
 
   // Update penalty parameters for next iteration.
   double next_muEq, next_muIneq;
@@ -186,7 +186,7 @@ NonlinearFactorGraph AugmentedLagrangianOptimizer::augmentedLagrangianFunction(
 /* ************************************************************************* */
 void AugmentedLagrangianOptimizer::updateLagrangeMultiplier(
     const State& previousState, State* state) const {
-  // Perform dual ascent on Lagrange multipliers for e-constriants.
+  // Perform dual ascent on Lagrange multipliers for e-constraints.
   const NonlinearEqualityConstraints& eqConstraints = problem_.eConstraints();
   state->lambdaEq.resize(eqConstraints.size());
   for (size_t i = 0; i < eqConstraints.size(); i++) {
@@ -198,7 +198,7 @@ void AugmentedLagrangianOptimizer::updateLagrangeMultiplier(
     state->lambdaEq[i] = previousState.lambdaEq[i] + step_size * violation;
   }
 
-  // Perform dual ascent on Lagrange multipliers for i-constriants.
+  // Perform dual ascent on Lagrange multipliers for i-constraints.
   const NonlinearInequalityConstraints& ineqConstraints =
       problem_.iConstraints();
   state->lambdaIneq.resize(ineqConstraints.size());
@@ -239,7 +239,7 @@ AugmentedLagrangianOptimizer::createUnconstrainedOptimizer(
     const NonlinearFactorGraph& graph, const Values& values) const {
   // TODO(yetong): make compatible with all NonlinearOptimizers.
   return std::make_shared<LevenbergMarquardtOptimizer>(graph, values,
-                                                       p_->lm_params);
+                                                       p_->lmParams);
 }
 
 /* ************************************************************************* */
@@ -283,7 +283,7 @@ void AugmentedLagrangianOptimizer::logIteration(const State& state) const {
     cout << "|" << setw(10) << setprecision(4) << state.cost;
     cout << "|" << setw(10) << setprecision(4) << state.eqConstraintViolation;
     cout << "|" << setw(10) << setprecision(4) << state.ineqConstraintViolation;
-    cout << "|" << setw(10) << state.unconstrainedIterationss;
+    cout << "|" << setw(10) << state.unconstrainedIterations;
     cout << "|" << setw(10) << state.time;
     cout << "|" << endl;
   }

gtsam/constrained/NonlinearEqualityConstraint.h

@@ -38,6 +38,9 @@ class GTSAM_EXPORT NonlinearEqualityConstraint : public NonlinearConstraint {
   /** Destructor. */
   virtual ~NonlinearEqualityConstraint() {}
 
+  /// Whether this constraint should be treated as a hard constraint.
+  virtual bool isHardConstraint() const { return true; }
+
  private:
 #if GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */

gtsam/constrained/PenaltyOptimizer.cpp

@@ -41,7 +41,7 @@ PenaltyOptimizer::State PenaltyOptimizer::iterate(const State& state) const {
   // Run unconstrained optimization.
   auto optimizer = createUnconstrainedOptimizer(meritGraph, state.values);
   newState.setValues(optimizer->optimize(), problem_);
-  newState.unconstrainedIterationss = optimizer->iterations();
+  newState.unconstrainedIterations = optimizer->iterations();
 
   return newState;
 }
@@ -78,7 +78,7 @@ PenaltyOptimizer::createUnconstrainedOptimizer(
     const NonlinearFactorGraph& graph, const Values& values) const {
   // TODO(yetong): make compatible with all NonlinearOptimizers.
   return std::make_shared<LevenbergMarquardtOptimizer>(graph, values,
-                                                       p_->lm_params);
+                                                       p_->lmParams);
 }
 
 /* ************************************************************************* */
@@ -122,7 +122,7 @@ void PenaltyOptimizer::logIteration(const State& state) const {
     cout << "|" << setw(10) << setprecision(4) << state.cost;
     cout << "|" << setw(10) << setprecision(4) << state.eqConstraintViolation;
     cout << "|" << setw(10) << setprecision(4) << state.ineqConstraintViolation;
-    cout << "|" << setw(10) << state.unconstrainedIterationss;
+    cout << "|" << setw(10) << state.unconstrainedIterations;
     cout << "|" << setw(10) << state.time;
     cout << "|" << endl;
   }

gtsam/constrained/PenaltyOptimizer.h

@@ -35,10 +35,13 @@ class GTSAM_EXPORT PenaltyOptimizerParams : public ConstrainedOptimizerParams {
   double muEqIncreaseRate = 2;  // increase rate of penalty parameter
   double muIneqIncreaseRate = 2;
   InequalityPenaltyFunction::shared_ptr ineqConstraintPenaltyFunction = nullptr;
-  LevenbergMarquardtParams lm_params;
+  LevenbergMarquardtParams lmParams;
 
   /** Constructor. */
-  PenaltyOptimizerParams() : Base() {}
+  PenaltyOptimizerParams() : Base() {
+    lmParams.linearSolverType =
+        LevenbergMarquardtParams::MULTIFRONTAL_CHOLESKY;
+  }
 };
 
 /// Details for each iteration.
@@ -50,7 +53,7 @@ class GTSAM_EXPORT PenaltyOptimizerState : public ConstrainedOptimizerState {
 
   double muEq = 0.0;
   double muIneq = 0.0;
-  size_t unconstrainedIterationss = 0;
+  size_t unconstrainedIterations = 0;
 
   using Base::Base;
 };

gtsam/constrained/tests/testAugmentedLagrangianOptimizer.cpp

@@ -10,8 +10,8 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file  testAugmentedLagrangianOptimizr.cpp
- * @brief Test augmented Lagrangian method optimzier for equality constrained
+ * @file  testAugmentedLagrangianOptimizer.cpp
+ * @brief Test augmented Lagrangian method optimizer for equality constrained
  * optimization.
  * @author: Yetong Zhang
  */