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

20
gtsam/nonlinear/ExpressionFactor.h
View File

@ -32,7 +32,7 @@ namespace gtsam {
template<class T> template<class T>
class ExpressionFactor: public NoiseModelFactor { class ExpressionFactor: public NoiseModelFactor {
protected: protected:
typedef ExpressionFactor<T> This; typedef ExpressionFactor<T> This;
@ -42,7 +42,7 @@ class ExpressionFactor: public NoiseModelFactor {
static const int Dim = traits<T>::dimension; static const int Dim = traits<T>::dimension;
public: public:
typedef boost::shared_ptr<ExpressionFactor<T> > shared_ptr; typedef boost::shared_ptr<ExpressionFactor<T> > shared_ptr;
@ -83,13 +83,16 @@ class ExpressionFactor: public NoiseModelFactor {
if (!active(x)) if (!active(x))
return boost::shared_ptr<JacobianFactor>(); return boost::shared_ptr<JacobianFactor>();
// Create a writeable JacobianFactor in advance
// In case noise model is constrained, we need to provide a noise model // 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( boost::shared_ptr<JacobianFactor> factor(
constrained ? new JacobianFactor(keys_, dims_, Dim, new JacobianFactor(keys_, dims_, Dim, noiseModel));
boost::static_pointer_cast<noiseModel::Constrained>(noiseModel_)->unit()) :
new JacobianFactor(keys_, dims_, Dim));
// Wrap keys and VerticalBlockMatrix into structure passed to expression_ // Wrap keys and VerticalBlockMatrix into structure passed to expression_
VerticalBlockMatrix& Ab = factor->matrixObject(); VerticalBlockMatrix& Ab = factor->matrixObject();
@ -114,7 +117,8 @@ class ExpressionFactor: public NoiseModelFactor {
/// @return a deep copy of this factor /// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const { virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); } gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
}; };
// ExpressionFactor // 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. // The JacobianMap provides a mapping from keys to the underlying blocks.
class JacobianMap { class JacobianMap {
private: private:
typedef FastVector<Key> Keys; const KeyVector& keys_;
const FastVector<Key>& keys_;
VerticalBlockMatrix& Ab_; VerticalBlockMatrix& Ab_;
public: public:
/// Construct a JacobianMap for writing into a VerticalBlockMatrix Ab /// 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) { keys_(keys), Ab_(Ab) {
} }
/// Access blocks of via key /// Access blocks of via key
VerticalBlockMatrix::Block operator()(Key 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(); DenseIndex block = it - keys_.begin();
return Ab_(block); return Ab_(block);
} }