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

20
gtsam/linear/JacobianFactor.cpp
View File

@ -304,16 +304,18 @@ 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();
this->getb().segment(nextRow, sourceRows) = jacobians[factorI]->getb(); if(sourceRows > 0) {
if(jacobians[factorI]->get_model()) { this->getb().segment(nextRow, sourceRows) = jacobians[factorI]->getb();
// If the factor has a noise model and we haven't yet allocated sigmas, allocate it. if(jacobians[factorI]->get_model()) {
if(!sigmas) // If the factor has a noise model and we haven't yet allocated sigmas, allocate it.
sigmas = Vector::Constant(m, 1.0); if(!sigmas)
sigmas->segment(nextRow, sourceRows) = jacobians[factorI]->get_model()->sigmas(); sigmas = Vector::Constant(m, 1.0);
if (jacobians[factorI]->isConstrained()) sigmas->segment(nextRow, sourceRows) = jacobians[factorI]->get_model()->sigmas();
anyConstrained = true; if (jacobians[factorI]->isConstrained())
anyConstrained = true;
}
nextRow += sourceRows;
} }
nextRow += sourceRows;
} }
gttoc(copy_vectors); gttoc(copy_vectors);