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

Fixed bug in new iSAM2 partial relinearization check that occurs when the Bayes Tree is empty

release/4.3a0
Stephen Williams 2012-06-29 15:09:03 +00:00
parent 0a66326891
commit 5ec2336f23
1 changed files with 7 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/nonlinear/ISAM2-impl.cpp
View File

@ -168,12 +168,13 @@ FastSet<Index> ISAM2::Impl::CheckRelinearizationPartial(const ISAM2Clique::share
FastSet<Index> relinKeys;
if(relinearizeThreshold.type() == typeid(double)) {
CheckRelinearizationRecursiveDouble(relinKeys, boost::get<double>(relinearizeThreshold), delta, root);
} else if(relinearizeThreshold.type() == typeid(FastMap<char,Vector>)) {
Ordering::InvertedMap decoder = ordering.invert();
CheckRelinearizationRecursiveMap(relinKeys, boost::get<FastMap<char,Vector> >(relinearizeThreshold), delta, decoder, root);
if(root) {
if(relinearizeThreshold.type() == typeid(double)) {
CheckRelinearizationRecursiveDouble(relinKeys, boost::get<double>(relinearizeThreshold), delta, root);
} else if(relinearizeThreshold.type() == typeid(FastMap<char,Vector>)) {
Ordering::InvertedMap decoder = ordering.invert();
CheckRelinearizationRecursiveMap(relinKeys, boost::get<FastMap<char,Vector> >(relinearizeThreshold), delta, decoder, root);
}
}
return relinKeys;