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

Fixed problem is JacobianFactor combine with zero-row factors

release/4.3a0
Richard Roberts 2013-08-06 23:55:38 +00:00
parent b2b7b0b3f3
commit 7aa1c61dcd
1 changed files with 11 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/linear/JacobianFactor.cpp
View File

@ -304,6 +304,7 @@ namespace gtsam {
DenseIndex nextRow = 0; DenseIndex nextRow = 0;
for(size_t factorI = 0; factorI < jacobians.size(); ++factorI) { for(size_t factorI = 0; factorI < jacobians.size(); ++factorI) {
const DenseIndex sourceRows = jacobians[factorI]->rows(); const DenseIndex sourceRows = jacobians[factorI]->rows();
if(sourceRows > 0) {
this->getb().segment(nextRow, sourceRows) = jacobians[factorI]->getb(); this->getb().segment(nextRow, sourceRows) = jacobians[factorI]->getb();
if(jacobians[factorI]->get_model()) { if(jacobians[factorI]->get_model()) {
// If the factor has a noise model and we haven't yet allocated sigmas, allocate it. // If the factor has a noise model and we haven't yet allocated sigmas, allocate it.
@ -315,6 +316,7 @@ namespace gtsam {
} }
nextRow += sourceRows; nextRow += sourceRows;
} }
}
gttoc(copy_vectors); gttoc(copy_vectors);
if(sigmas) if(sigmas)