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

A zero noiseModel_ never worked for NoiseModelFactor, regularizing this by explicit check

release/4.3a0
dellaert 2014-10-02 11:01:14 +02:00
parent 1dddb4046a
commit 8a196eb86e
1 changed files with 26 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
gtsam/nonlinear/NonlinearFactor.cpp
View File

@ -73,20 +73,26 @@ bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const {
} }
Vector NoiseModelFactor::whitenedError(const Values& c) const { Vector NoiseModelFactor::whitenedError(const Values& c) const {
const Vector unwhitenedErrorVec = unwhitenedError(c); const Vector b = unwhitenedError(c);
if ((size_t) unwhitenedErrorVec.size() != noiseModel_->dim()) if ((size_t) b.size() != noiseModel_->dim())
throw std::invalid_argument( throw std::invalid_argument(
"This factor was created with a NoiseModel of incorrect dimension."); "This factor was created with a NoiseModel of incorrect dimension.");
return noiseModel_->whiten(unwhitenedErrorVec); return noiseModel_->whiten(b);
}
static void check(const SharedNoiseModel& noiseModel, const Vector& b) {
if (!noiseModel)
throw std::invalid_argument("NoiseModelFactor: no NoiseModel.");
if ((size_t) b.size() != noiseModel->dim())
throw std::invalid_argument(
"NoiseModelFactor was created with a NoiseModel of incorrect dimension.");
} }
double NoiseModelFactor::error(const Values& c) const { double NoiseModelFactor::error(const Values& c) const {
if (this->active(c)) { if (this->active(c)) {
const Vector unwhitenedErrorVec = unwhitenedError(c); const Vector b = unwhitenedError(c);
if ((size_t) unwhitenedErrorVec.size() != noiseModel_->dim()) check(noiseModel_, b);
throw std::invalid_argument( return 0.5 * noiseModel_->distance(b);
"This factor was created with a NoiseModel of incorrect dimension.");
return 0.5 * noiseModel_->distance(unwhitenedErrorVec);
} else { } else {
return 0.0; return 0.0;
} }
@ -102,14 +108,10 @@ boost::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
// Call evaluate error to get Jacobians and RHS vector b // Call evaluate error to get Jacobians and RHS vector b
std::vector<Matrix> A(this->size()); std::vector<Matrix> A(this->size());
Vector b = -unwhitenedError(x, A); Vector b = -unwhitenedError(x, A);
check(noiseModel_, b);
// If a noiseModel is given, whiten the corresponding system now // Whiten the corresponding system now
if (noiseModel_) {
if ((size_t) b.size() != noiseModel_->dim())
throw std::invalid_argument(
"This factor was created with a NoiseModel of incorrect dimension.");
this->noiseModel_->WhitenSystem(A, b); this->noiseModel_->WhitenSystem(A, b);
}
// Fill in terms, needed to create JacobianFactor below // Fill in terms, needed to create JacobianFactor below
std::vector<std::pair<Key, Matrix> > terms(this->size()); std::vector<std::pair<Key, Matrix> > terms(this->size());
@ -120,8 +122,7 @@ boost::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
// TODO pass unwhitened + noise model to Gaussian factor // TODO pass unwhitened + noise model to Gaussian factor
// For now, only linearized constrained factors have noise model at linear level!!! // For now, only linearized constrained factors have noise model at linear level!!!
if (noiseModel_) { noiseModel::Constrained::shared_ptr constrained = //
noiseModel::Constrained::shared_ptr constrained =
boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_); boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
if (constrained) { if (constrained) {
// Create a factor of reduced row dimension d_ // Create a factor of reduced row dimension d_
@ -132,8 +133,6 @@ boost::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
return boost::make_shared<JacobianFactor>(terms, b_, model); return boost::make_shared<JacobianFactor>(terms, b_, model);
} else } else
return GaussianFactor::shared_ptr(new JacobianFactor(terms, b)); return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
} else
return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
} }
/* ************************************************************************* */ /* ************************************************************************* */