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