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

support special linearize for constraints, which Jacobian could be augmented by Jacobian-from-Hessian parts for Lagrange multipliers in SQP

release/4.3a0
Duy-Nguyen Ta 2013-10-02 03:55:11 +00:00
parent 63ff1b47c1
commit c3e3b693e7
1 changed files with 7 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/nonlinear/NonlinearFactor.h
View File

@ -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<noiseModel::Constrained>(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));
}