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

Modified finding method

release/4.3a0
Sungtae An 2014-10-31 11:06:26 -04:00
parent 6a20d35a60
commit a5b8d0fd35
1 changed files with 12 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
gtsam_unstable/nonlinear/Expression-inl.h
View File

@ -26,6 +26,8 @@
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include <boost/bind.hpp>
#include <boost/lambda/lambda.hpp>
// template meta-programming headers // template meta-programming headers
#include <boost/mpl/vector.hpp> #include <boost/mpl/vector.hpp>
@ -48,7 +50,8 @@ namespace gtsam {
template<typename T> template<typename T>
class Expression; class Expression;
typedef std::vector<std::pair<Key, Eigen::Block<Matrix> > > JacobianMap; typedef std::pair<Key, Eigen::Block<Matrix> > JacobianPair;
typedef std::vector<JacobianPair> JacobianMap;
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
/** /**
@ -78,28 +81,20 @@ struct CallRecord {
template<int ROWS, int COLS> template<int ROWS, int COLS>
void handleLeafCase(const Eigen::Matrix<double, ROWS, COLS>& dTdA, void handleLeafCase(const Eigen::Matrix<double, ROWS, COLS>& dTdA,
JacobianMap& jacobians, Key key) { JacobianMap& jacobians, Key key) {
for(JacobianMap::iterator it = jacobians.begin(); it != jacobians.end(); ++it) JacobianMap::iterator it = std::find_if(jacobians.begin(), jacobians.end(),
{ boost::bind(&JacobianPair::first, _1)==key);
if(it->first == key) assert(it~= jacobians.end());
{
it->second.block<ROWS, COLS>(0, 0) += dTdA; // block makes HUGE difference it->second.block<ROWS, COLS>(0, 0) += dTdA; // block makes HUGE difference
break;
}
}
} }
/// Handle Leaf Case for Dynamic Matrix type (slower) /// Handle Leaf Case for Dynamic Matrix type (slower)
template<> template<>
void handleLeafCase( void handleLeafCase(
const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>& dTdA, const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>& dTdA,
JacobianMap& jacobians, Key key) { JacobianMap& jacobians, Key key) {
for(JacobianMap::iterator it = jacobians.begin(); it != jacobians.end(); ++it) JacobianMap::iterator it = std::find_if(jacobians.begin(), jacobians.end(),
{ boost::bind(&JacobianPair::first, _1)==key);
if(it->first == key) assert(it~= jacobians.end());
{ it->second += dTdA;
it->second += dTdA; // block makes HUGE difference
break;
}
}
} }
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------