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

Changed the type of JacobianMap as std::vector

release/4.3a0
Sungtae An 2014-10-31 07:10:53 -04:00
parent c1c6a30e50
commit 97d4120858
2 changed files with 22 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -48,7 +48,8 @@ namespace gtsam {
template<typename T> template<typename T>
class Expression; class Expression;
typedef std::map<Key, Eigen::Block<Matrix> > JacobianMap; //typedef std::map<Key, Eigen::Block<Matrix> > JacobianMap;
typedef std::vector<std::pair<Key, Eigen::Block<Matrix> > > JacobianMap;
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
/** /**
@ -78,16 +79,28 @@ 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) {
JacobianMap::iterator it = jacobians.find(key); for(JacobianMap::iterator it = jacobians.begin(); it != jacobians.end(); ++it)
it->second.block<ROWS, COLS>(0, 0) += dTdA; // block makes HUGE difference {
if((*it).first == key)
{
(*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) {
JacobianMap::iterator it = jacobians.find(key); for(JacobianMap::iterator it = jacobians.begin(); it != jacobians.end(); ++it)
it->second += dTdA; {
if((*it).first == key)
{
(*it).second += dTdA; // block makes HUGE difference
break;
}
}
} }
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------

6
gtsam_unstable/nonlinear/ExpressionFactor.h
View File

@ -87,12 +87,13 @@ public:
// Create and zero out blocks to be passed to expression_ // Create and zero out blocks to be passed to expression_
JacobianMap blocks; JacobianMap blocks;
blocks.reserve(size());
for (DenseIndex i = 0; i < size(); i++) { for (DenseIndex i = 0; i < size(); i++) {
Matrix& Hi = H->at(i); Matrix& Hi = H->at(i);
Hi.resize(Dim, dimensions_[i]); Hi.resize(Dim, dimensions_[i]);
Hi.setZero(); // zero out Hi.setZero(); // zero out
Eigen::Block<Matrix> block = Hi.block(0, 0, Dim, dimensions_[i]); Eigen::Block<Matrix> block = Hi.block(0, 0, Dim, dimensions_[i]);
blocks.insert(std::make_pair(keys_[i], block)); blocks.push_back(std::make_pair(keys_[i], block));
} }
T value = expression_.value(x, blocks); T value = expression_.value(x, blocks);
@ -121,8 +122,9 @@ public:
// Create blocks into Ab_ to be passed to expression_ // Create blocks into Ab_ to be passed to expression_
JacobianMap blocks; JacobianMap blocks;
blocks.reserve(size());
for (DenseIndex i = 0; i < size(); i++) for (DenseIndex i = 0; i < size(); i++)
blocks.insert(std::make_pair(keys_[i], Ab(i))); blocks.push_back(std::make_pair(keys_[i], Ab(i)));
// Evaluate error to get Jacobians and RHS vector b // Evaluate error to get Jacobians and RHS vector b
T value = expression_.value(x, blocks); // <<< Reverse AD happens here ! T value = expression_.value(x, blocks); // <<< Reverse AD happens here !