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

size_t argument for check

release/4.3a0
dellaert 2014-10-14 11:13:09 +02:00
parent 4c76f39009
commit 027759300d
1 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/nonlinear/NonlinearFactor.cpp
View File

@ -72,24 +72,24 @@ bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const {
&& noiseModel_->equals(*e->noiseModel_, tol))); && noiseModel_->equals(*e->noiseModel_, tol)));
} }
static void check(const SharedNoiseModel& noiseModel, const Vector& b) { static void check(const SharedNoiseModel& noiseModel, size_t m) {
if (!noiseModel) if (!noiseModel)
throw std::invalid_argument("NoiseModelFactor: no NoiseModel."); throw std::invalid_argument("NoiseModelFactor: no NoiseModel.");
if ((size_t) b.size() != noiseModel->dim()) if (m != noiseModel->dim())
throw std::invalid_argument( throw std::invalid_argument(
"NoiseModelFactor was created with a NoiseModel of incorrect dimension."); "NoiseModelFactor was created with a NoiseModel of incorrect dimension.");
} }
Vector NoiseModelFactor::whitenedError(const Values& c) const { Vector NoiseModelFactor::whitenedError(const Values& c) const {
const Vector b = unwhitenedError(c); const Vector b = unwhitenedError(c);
check(noiseModel_, b); check(noiseModel_, b.size());
return noiseModel_->whiten(b); return noiseModel_->whiten(b);
} }
double NoiseModelFactor::error(const Values& c) const { double NoiseModelFactor::error(const Values& c) const {
if (this->active(c)) { if (this->active(c)) {
const Vector b = unwhitenedError(c); const Vector b = unwhitenedError(c);
check(noiseModel_, b); check(noiseModel_, b.size());
return 0.5 * noiseModel_->distance(b); return 0.5 * noiseModel_->distance(b);
} else { } else {
return 0.0; return 0.0;
@ -106,7 +106,7 @@ 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); check(noiseModel_, b.size());
// Whiten the corresponding system now // Whiten the corresponding system now
this->noiseModel_->WhitenSystem(A, b); this->noiseModel_->WhitenSystem(A, b);