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

Missed checks for empty noise model in GC and JF

release/4.3a0
Richard Roberts 2013-07-16 20:22:10 +00:00
parent e846ed119c
commit cd97f9866a
2 changed files with 15 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/linear/GaussianConditionalUnordered.cpp
View File

@ -140,7 +140,8 @@ namespace gtsam {
Vector soln = get_R().triangularView<Eigen::Upper>().solve(xS); Vector soln = get_R().triangularView<Eigen::Upper>().solve(xS);
// Scale by sigmas // Scale by sigmas
soln.array() *= model_->sigmas().array(); if(model_)
soln.array() *= model_->sigmas().array();
// Insert solution into a VectorValues // Insert solution into a VectorValues
VectorValuesUnordered result; VectorValuesUnordered result;

18
gtsam/linear/JacobianFactorUnordered.cpp
View File

@ -442,21 +442,29 @@ namespace gtsam {
Matrix A(Ab_.range(0, size())); Matrix A(Ab_.range(0, size()));
Vector b(getb()); Vector b(getb());
// divide in sigma so error is indeed 0.5*|Ax-b| // divide in sigma so error is indeed 0.5*|Ax-b|
if (weight) model_->WhitenSystem(A,b); if (weight && model_)
model_->WhitenSystem(A,b);
return make_pair(A, b); return make_pair(A, b);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix JacobianFactorUnordered::augmentedJacobian(bool weight) const { Matrix JacobianFactorUnordered::augmentedJacobian(bool weight) const {
if (weight) { Matrix Ab(Ab_.range(0,Ab_.nBlocks())); model_->WhitenInPlace(Ab); return Ab; } if (weight && model_) {
else return Ab_.range(0, Ab_.nBlocks()); Matrix Ab(Ab_.range(0,Ab_.nBlocks()));
model_->WhitenInPlace(Ab);
return Ab;
} else {
return Ab_.range(0, Ab_.nBlocks());
}
} }
/* ************************************************************************* */ /* ************************************************************************* */
JacobianFactorUnordered JacobianFactorUnordered::whiten() const { JacobianFactorUnordered JacobianFactorUnordered::whiten() const {
JacobianFactorUnordered result(*this); JacobianFactorUnordered result(*this);
result.model_->WhitenInPlace(result.Ab_.matrix()); if(model_) {
result.model_ = noiseModel::Unit::Create(result.model_->dim()); result.model_->WhitenInPlace(result.Ab_.matrix());
result.model_ = noiseModel::Unit::Create(result.model_->dim());
}
return result; return result;
} }