From c3e3b693e74c2308af42d9beabead79419eeddb0 Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Wed, 2 Oct 2013 03:55:11 +0000 Subject: [PATCH] support special linearize for constraints, which Jacobian could be augmented by Jacobian-from-Hessian parts for Lagrange multipliers in SQP --- gtsam/nonlinear/NonlinearFactor.h | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 010341804..3df049268 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -315,10 +315,15 @@ public: } // TODO pass unwhitened + noise model to Gaussian factor + // For now, only linearized constrained factors have noise model at linear level!!! noiseModel::Constrained::shared_ptr constrained = boost::dynamic_pointer_cast(this->noiseModel_); - if(constrained) - return GaussianFactor::shared_ptr(new JacobianFactor(terms, b, constrained->unit())); + if(constrained) { + size_t augmentedDim = terms[0].second.rows() - constrained->dim(); + Vector augmentedZero = zero(augmentedDim); + Vector augmentedb = concatVectors(2, &b, &augmentedZero); + return GaussianFactor::shared_ptr(new JacobianFactor(terms, augmentedb, constrained->unit(augmentedDim))); + } else return GaussianFactor::shared_ptr(new JacobianFactor(terms, b)); }