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

Allowing empty noise model in NonlinearFactor

release/4.3a0
Richard Roberts 2013-12-28 12:26:24 -05:00
parent 3b71fe47bc
commit 2805ecfe1c
1 changed files with 17 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/nonlinear/NonlinearFactor.h
View File

@ -304,10 +304,13 @@ public:
// Call evaluate error to get Jacobians and b vector // Call evaluate error to get Jacobians and b vector
std::vector<Matrix> A(this->size()); std::vector<Matrix> A(this->size());
b = -unwhitenedError(x, A); b = -unwhitenedError(x, A);
if(noiseModel_)
{
if((size_t) b.size() != noiseModel_->dim()) if((size_t) b.size() != noiseModel_->dim())
throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension."); throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension.");
this->noiseModel_->WhitenSystem(A,b); this->noiseModel_->WhitenSystem(A,b);
}
std::vector<std::pair<Key, Matrix> > terms(this->size()); std::vector<std::pair<Key, Matrix> > terms(this->size());
// Fill in terms // Fill in terms
@ -318,6 +321,8 @@ public:
// 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) {
@ -329,6 +334,9 @@ public:
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));
}
private: private: