diff --git a/gtsam/linear/linearAlgorithms-inst.h b/gtsam/linear/linearAlgorithms-inst.h index 4ec8c9a07..181d8028a 100644 --- a/gtsam/linear/linearAlgorithms-inst.h +++ b/gtsam/linear/linearAlgorithms-inst.h @@ -77,8 +77,9 @@ namespace gtsam xS.resize(dim); DenseIndex vectorPos = 0; BOOST_FOREACH(const VectorValues::const_iterator& parentPointer, parentPointers) { - xS.segment(vectorPos, parentPointer->second.size()) = parentPointer->second; - vectorPos += parentPointer->second.size(); + const Vector& parentVector = parentPointer->second; + xS.block(vectorPos,0,parentVector.size(),1) = parentVector.block(0,0,parentVector.size(),1); + vectorPos += parentVector.size(); } } xS = c.getb() - c.get_S() * xS; diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index d41100ea2..0d33308fb 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -183,8 +183,9 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh xS.resize(dim); DenseIndex vectorPos = 0; BOOST_FOREACH(const VectorValues::const_iterator& parentPointer, parentPointers) { - xS.segment(vectorPos, parentPointer->second.size()) = parentPointer->second; - vectorPos += parentPointer->second.size(); + const Vector& parentVector = parentPointer->second; + xS.block(vectorPos,0,parentVector.size(),1) = parentVector.block(0,0,parentVector.size(),1); + vectorPos += parentVector.size(); } } xS = c.getb() - c.get_S() * xS;