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

Fixed asserts

release/4.3a0
dellaert 2014-10-31 16:29:15 +01:00
parent d0c3bc0c8e
commit 593edd1e5c
1 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -81,9 +81,9 @@ 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) {
JacobianMap::iterator it = std::find_if(jacobians.begin(), jacobians.end(), JacobianMap::iterator it = std::find_if(jacobians.begin(), jacobians.end(),
boost::bind(&JacobianPair::first, _1)==key); boost::bind(&JacobianPair::first, _1) == key);
assert(it~= jacobians.end()); 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
} }
/// Handle Leaf Case for Dynamic Matrix type (slower) /// Handle Leaf Case for Dynamic Matrix type (slower)
template<> template<>
@ -91,8 +91,8 @@ 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) {
JacobianMap::iterator it = std::find_if(jacobians.begin(), jacobians.end(), JacobianMap::iterator it = std::find_if(jacobians.begin(), jacobians.end(),
boost::bind(&JacobianPair::first, _1)==key); boost::bind(&JacobianPair::first, _1) == key);
assert(it~= jacobians.end()); assert(it!=jacobians.end());
it->second += dTdA; it->second += dTdA;
} }