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

Now overwriting linearize as preparation for direct VericalBlockMatrix

release/4.3a0
dellaert 2014-10-11 17:05:53 +02:00
parent 1bac83381f
commit 0c7ea68f2f
2 changed files with 32 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/nonlinear/NonlinearFactor.cpp
View File

@ -124,7 +124,7 @@ boost::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
if (constrained) {
// Create a factor of reduced row dimension d_
size_t d_ = terms[0].second.rows() - constrained->dim();
size_t d_ = b.size() - constrained->dim();
Vector zero_ = zero(d_);
Vector b_ = concatVectors(2, &b, &zero_);
noiseModel::Constrained::shared_ptr model = constrained->unit(d_);

31
gtsam_unstable/nonlinear/ExpressionFactor.h
View File

@ -60,6 +60,37 @@ public:
}
}
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
// Only linearize if the factor is active
if (!this->active(x))
return boost::shared_ptr<JacobianFactor>();
// Evaluate error to get Jacobians and RHS vector b
Augmented<T> augmented = expression_.augmented(x);
Vector b = - measurement_.localCoordinates(augmented.value());
// check(noiseModel_, b); // TODO: use, but defined in NonlinearFactor.cpp
// Whiten the corresponding system now
// TODO ! this->noiseModel_->WhitenSystem(A, b);
// Terms, needed to create JacobianFactor below, are already here!
const JacobianMap& terms = augmented.jacobians();
// TODO pass unwhitened + noise model to Gaussian factor
// For now, only linearized constrained factors have noise model at linear level!!!
noiseModel::Constrained::shared_ptr constrained = //
boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
if (constrained) {
// Create a factor of reduced row dimension d_
size_t d_ = b.size() - constrained->dim();
Vector zero_ = zero(d_);
Vector b_ = concatVectors(2, &b, &zero_);
noiseModel::Constrained::shared_ptr model = constrained->unit(d_);
return boost::make_shared<JacobianFactor>(terms, b_, model);
} else
return boost::make_shared<JacobianFactor>(terms, b);
}
};
// ExpressionFactor