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

bug fix: before hessianDiagonal seg-faulted with no noise model

release/4.3a0
Luca 2014-02-15 16:44:20 -05:00
parent ba6f8b7cca
commit 71e4015304
2 changed files with 7 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/linear/JacobianFactor.cpp
View File

@ -447,7 +447,11 @@ namespace gtsam {
size_t nj = Ab_(pos).cols(); size_t nj = Ab_(pos).cols();
Vector dj(nj); Vector dj(nj);
for (size_t k = 0; k < nj; ++k) { for (size_t k = 0; k < nj; ++k) {
Vector column_k = model_->whiten(Ab_(pos).col(k)); Vector column_k;
if(model_)
column_k = model_->whiten(Ab_(pos).col(k));
else
column_k = Ab_(pos).col(k);
dj(k) = dot(column_k,column_k); dj(k) = dot(column_k,column_k);
} }
d.insert(j,dj); d.insert(j,dj);

2
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -113,7 +113,9 @@ void LevenbergMarquardtOptimizer::iterate() {
double sigma = 1.0 / std::sqrt(state_.lambda); double sigma = 1.0 / std::sqrt(state_.lambda);
dampedSystem.reserve(dampedSystem.size() + state_.values.size()); dampedSystem.reserve(dampedSystem.size() + state_.values.size());
// for each of the variables, add a prior // for each of the variables, add a prior
VectorValues diagHessian = linear->hessianDiagonal();
BOOST_FOREACH(const Values::KeyValuePair& key_value, state_.values) { BOOST_FOREACH(const Values::KeyValuePair& key_value, state_.values) {
size_t dim = key_value.value.dim(); size_t dim = key_value.value.dim();
Matrix A = Matrix::Identity(dim, dim); Matrix A = Matrix::Identity(dim, dim);
Vector b = Vector::Zero(dim); Vector b = Vector::Zero(dim);