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

Slight refactor/reformatting

release/4.3a0
dellaert 2015-07-08 19:14:23 -07:00
parent 61d6ba8a57
commit 0c29215018
2 changed files with 15 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/nonlinear/ExpressionFactor.h
View File

@ -83,13 +83,16 @@ class ExpressionFactor: public NoiseModelFactor {
if (!active(x))
return boost::shared_ptr<JacobianFactor>();
// Create a writeable JacobianFactor in advance
// In case noise model is constrained, we need to provide a noise model
bool constrained = noiseModel_->isConstrained();
SharedDiagonal noiseModel;
if (noiseModel_->isConstrained()) {
noiseModel = boost::static_pointer_cast<noiseModel::Constrained>(
noiseModel_)->unit();
}
// Create a writeable JacobianFactor in advance
boost::shared_ptr<JacobianFactor> factor(
constrained ? new JacobianFactor(keys_, dims_, Dim,
boost::static_pointer_cast<noiseModel::Constrained>(noiseModel_)->unit()) :
new JacobianFactor(keys_, dims_, Dim));
new JacobianFactor(keys_, dims_, Dim, noiseModel));
// Wrap keys and VerticalBlockMatrix into structure passed to expression_
VerticalBlockMatrix& Ab = factor->matrixObject();
@ -114,7 +117,8 @@ class ExpressionFactor: public NoiseModelFactor {
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
};
// ExpressionFactor

7
gtsam/nonlinear/internal/JacobianMap.h
View File

@ -31,19 +31,18 @@ namespace internal {
// The JacobianMap provides a mapping from keys to the underlying blocks.
class JacobianMap {
private:
typedef FastVector<Key> Keys;
const FastVector<Key>& keys_;
const KeyVector& keys_;
VerticalBlockMatrix& Ab_;
public:
/// Construct a JacobianMap for writing into a VerticalBlockMatrix Ab
JacobianMap(const Keys& keys, VerticalBlockMatrix& Ab) :
JacobianMap(const KeyVector& keys, VerticalBlockMatrix& Ab) :
keys_(keys), Ab_(Ab) {
}
/// Access blocks of via key
VerticalBlockMatrix::Block operator()(Key key) {
Keys::const_iterator it = std::find(keys_.begin(), keys_.end(), key);
KeyVector::const_iterator it = std::find(keys_.begin(), keys_.end(), key);
DenseIndex block = it - keys_.begin();
return Ab_(block);
}