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

NonlinearFactors handle constrained noise models properly now, as before they would try to whiten and fail

release/4.3a0
Alex Cunningham 2010-01-28 17:21:24 +00:00
parent 06a062c931
commit eb88a149a1
3 changed files with 57 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
cpp/NoiseModel.h
View File

@ -164,6 +164,11 @@ virtual SharedDiagonal QR(Matrix& Ab) const;
*/
virtual Matrix R() const { return thisR();}
/**
* Simple check for constrained-ness
*/
virtual bool isConstrained() const {return false;}
}; // Gaussian
@ -232,12 +237,6 @@ public:
virtual Matrix R() const {
return diag(invsigmas_);
}
/**
* Simple check for constrained-ness
*/
virtual bool isConstrained() {return false;}
}; // Diagonal
/**
@ -308,9 +307,9 @@ public:
virtual SharedDiagonal QR(Matrix& Ab) const;
/**
* Not constrained
* Check constrained is always true
*/
virtual bool isConstrained() {return true;}
virtual bool isConstrained() const {return true;}
}; // Constrained

11
cpp/NonlinearFactor.h
View File

@ -190,6 +190,11 @@ namespace gtsam {
Matrix A;
Vector b = - evaluateError(xj, A);
// TODO pass unwhitened + noise model to Gaussian factor
SharedDiagonal constrained =
boost::shared_dynamic_cast<noiseModel::Constrained>(this->noiseModel_);
if (constrained.get() != NULL) {
return GaussianFactor::shared_ptr(new GaussianFactor(key_, A, b, constrained));
}
this->noiseModel_->WhitenInPlace(A);
this->noiseModel_->whitenInPlace(b);
return GaussianFactor::shared_ptr(new GaussianFactor(key_, A, b,
@ -288,6 +293,12 @@ namespace gtsam {
Matrix A1, A2;
Vector b = -evaluateError(x1, x2, A1, A2);
// TODO pass unwhitened + noise model to Gaussian factor
SharedDiagonal constrained =
boost::shared_dynamic_cast<noiseModel::Constrained>(this->noiseModel_);
if (constrained.get() != NULL) {
return GaussianFactor::shared_ptr(new GaussianFactor(key1_, A1, key2_,
A2, b, constrained));
}
this->noiseModel_->WhitenInPlace(A1);
this->noiseModel_->WhitenInPlace(A2);
this->noiseModel_->whitenInPlace(b);

39
cpp/testNonlinearFactor.cpp
View File

@ -175,6 +175,45 @@ TEST( NonlinearFactor, size )
CHECK(factor3->size() == 2);
}
/* ************************************************************************* */
TEST( NonlinearFactor, linearize_constraint1 )
{
Vector sigmas = Vector_(2, 0.2, 0.0);
SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas);
Point2 mu(1., -1.);
Graph::sharedFactor f0(new simulated2D::Prior(mu, constraint, 1));
Config config;
config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0));
GaussianFactor::shared_ptr actual = f0->linearize(config);
// create expected
Vector b = Vector_(2, 0., -3.);
GaussianFactor expected("x1", eye(2), b, constraint);
CHECK(assert_equal(expected, *actual));
}
/* ************************************************************************* */
TEST( NonlinearFactor, linearize_constraint2 )
{
Vector sigmas = Vector_(2, 0.2, 0.0);
SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas);
Point2 z3(1.,-1.);
simulated2D::Measurement f0(z3, constraint, 1,1);
Config config;
config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0));
config.insert(simulated2D::PointKey(1), Point2(5.0, 4.0));
GaussianFactor::shared_ptr actual = f0.linearize(config);
// create expected
Vector b = Vector_(2, -3., -3.);
GaussianFactor expected("x1", -1*eye(2), "l1", eye(2), b, constraint);
CHECK(assert_equal(expected, *actual));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */