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

fixed bug when reorder/relinearize is called with no factors

release/4.3a0
Julian Straub 2011-02-16 22:59:09 +00:00
parent ff0cb3eafb
commit 02fe443b44
1 changed files with 14 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
gtsam/nonlinear/NonlinearISAM-inl.h
View File

@ -64,23 +64,25 @@ void NonlinearISAM<Values>::reorder_relinearize() {
// cout << "Reordering, relinearizing..." << endl; // cout << "Reordering, relinearizing..." << endl;
// Obtain the new linearization point if(factors_.size() > 0) {
const Values newLinPoint = estimate(); // Obtain the new linearization point
const Values newLinPoint = estimate();
isam_.clear(); isam_.clear();
// Compute an ordering // Compute an ordering
ordering_ = *factors_.orderingCOLAMD(newLinPoint); ordering_ = *factors_.orderingCOLAMD(newLinPoint);
// Create a linear factor graph at the new linearization point // Create a linear factor graph at the new linearization point
boost::shared_ptr<GaussianFactorGraph> gfg( boost::shared_ptr<GaussianFactorGraph> gfg(
factors_.linearize(newLinPoint, ordering_)->template dynamicCastFactors<GaussianFactorGraph>()); factors_.linearize(newLinPoint, ordering_)->template dynamicCastFactors<GaussianFactorGraph>());
// Just recreate the whole BayesTree // Just recreate the whole BayesTree
isam_.update(*gfg); isam_.update(*gfg);
// Update linearization point // Update linearization point
linPoint_ = newLinPoint; linPoint_ = newLinPoint;
}
} }
/* ************************************************************************* */ /* ************************************************************************* */