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

Smart named constructors Covariance/Variances/Variance

release/4.3a0
Frank Dellaert 2010-01-20 00:26:49 +00:00
parent f577b27f52
commit b47438a86c
5 changed files with 97 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
cpp/GaussianFactor.h
View File

@ -60,7 +60,7 @@ public:
As_.insert(make_pair(key1, A1));
}
/** Construct unary factor with vector of sigmas*/
/** Construct unary factor with vector of sigmas */
GaussianFactor(const Symbol& key1, const Matrix& A1,
const Vector& b, const Vector& sigmas) :
b_(b), model_(noiseModel::Diagonal::Sigmas(sigmas)) {
@ -76,6 +76,15 @@ public:
As_.insert(make_pair(key2, A2));
}
/** Construct binary factor with vector of sigmas */
GaussianFactor(const Symbol& key1, const Matrix& A1,
const Symbol& key2, const Matrix& A2,
const Vector& b, const Vector& sigmas) :
b_(b), model_(noiseModel::Diagonal::Sigmas(sigmas)) {
As_.insert(make_pair(key1, A1));
As_.insert(make_pair(key2, A2));
}
/** Construct ternary factor */
GaussianFactor(const Symbol& key1, const Matrix& A1,
const Symbol& key2, const Matrix& A2,

51
cpp/NoiseModel.cpp
View File

@ -8,6 +8,8 @@
#include <limits>
#include <iostream>
#include <typeinfo>
#include <stdexcept>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/foreach.hpp>
@ -23,13 +25,30 @@ namespace gtsam {
namespace noiseModel {
/* ************************************************************************* */
void Gaussian::print(const string& name) const {
Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, bool smart) {
size_t m = covariance.size1(), n = covariance.size2();
if (m != n) throw invalid_argument("Gaussian::Covariance: covariance not square");
if (smart) {
// check all non-diagonal entries
int i,j;
for (i = 0; i < m; i++)
for (j = 0; j < n; j++)
if (i != j && fabs(covariance(i, j) > 1e-9)) goto full;
Vector variances(n);
for (j = 0; j < n; j++) variances(j) = covariance(j,j);
return Diagonal::Variances(variances,true);
}
full: return shared_ptr(new Gaussian(inverse_square_root(covariance)));
}
void Gaussian::print(const string& name) const {
gtsam::print(sqrt_information_, "Gaussian");
}
bool Gaussian::equals(const Base& m, double tol) const {
const Gaussian* p = dynamic_cast<const Gaussian*> (&m);
bool Gaussian::equals(const Base& expected, double tol) const {
const Gaussian* p = dynamic_cast<const Gaussian*> (&expected);
if (p == NULL) return false;
if (typeid(*this) != typeid(*p)) return false;
return equal_with_abs_tol(sqrt_information_, p->sqrt_information_, sqrt(tol));
}
@ -76,11 +95,22 @@ namespace gtsam {
/* ************************************************************************* */
// TODO: can we avoid calling reciprocal twice ?
Diagonal::Diagonal(const Vector& sigmas) :
Gaussian(diag(reciprocal(sigmas))), invsigmas_(reciprocal(sigmas)),
sigmas_(sigmas) {
}
Gaussian(diag(reciprocal(sigmas))), invsigmas_(reciprocal(sigmas)),
sigmas_(sigmas) {
}
void Diagonal::print(const string& name) const {
Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) {
if (smart) {
// check whether all the same entry
int j, n = variances.size();
for (j = 1; j < n; j++)
if (variances(j) != variances(0)) goto full;
return Isotropic::Variance(n, variances(0), true);
}
full: return shared_ptr(new Diagonal(esqrt(variances)));
}
void Diagonal::print(const string& name) const {
gtsam::print(sigmas_, "Diagonal sigmas " + name);
}
@ -205,7 +235,12 @@ namespace gtsam {
/* ************************************************************************* */
void Isotropic::print(const string& name) const {
Isotropic::shared_ptr Isotropic::Variance(size_t dim, double variance, bool smart) {
if (smart && fabs(variance-1.0)<1e-9) return Unit::Create(dim);
return shared_ptr(new Isotropic(dim, sqrt(variance)));
}
void Isotropic::print(const string& name) const {
cout << "Isotropic sigma " << name << " " << sigma_ << endl;
}

16
cpp/NoiseModel.h
View File

@ -85,6 +85,7 @@ namespace gtsam { namespace noiseModel {
/**
* A Gaussian noise model created by specifying a square root information matrix.
* @param smart, check if can be simplified to derived class
*/
static shared_ptr SqrtInformation(const Matrix& R) {
return shared_ptr(new Gaussian(R));
@ -92,10 +93,9 @@ namespace gtsam { namespace noiseModel {
/**
* A Gaussian noise model created by specifying a covariance matrix.
* @param smart, check if can be simplified to derived class
*/
static shared_ptr Covariance(const Matrix& covariance) {
return shared_ptr(new Gaussian(inverse_square_root(covariance)));
}
static shared_ptr Covariance(const Matrix& covariance, bool smart=false);
/**
* A Gaussian noise model created by specifying an information matrix.
@ -183,10 +183,9 @@ namespace gtsam { namespace noiseModel {
/**
* A diagonal noise model created by specifying a Vector of variances, i.e.
* i.e. the diagonal of the covariance matrix.
* @param smart, check if can be simplified to derived class
*/
static shared_ptr Variances(const Vector& variances) {
return shared_ptr(new Diagonal(esqrt(variances)));
}
static shared_ptr Variances(const Vector& variances, bool smart = false);
/**
* A diagonal noise model created by specifying a Vector of precisions, i.e.
@ -288,10 +287,9 @@ namespace gtsam { namespace noiseModel {
/**
* An isotropic noise model created by specifying a variance = sigma^2.
* @param smart, check if can be simplified to derived class
*/
static shared_ptr Variance(size_t dim, double variance) {
return shared_ptr(new Isotropic(dim, sqrt(variance)));
}
static shared_ptr Variance(size_t dim, double variance, bool smart = false);
/**
* An isotropic noise model created by specifying a precision

85
cpp/testGaussianFactor.cpp
View File

@ -221,36 +221,16 @@ TEST( NonlinearFactorGraph, combine2){
/* ************************************************************************* */
TEST( GaussianFactor, linearFactorN){
Matrix I = eye(2);
vector<GaussianFactor::shared_ptr> f;
f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1", Matrix_(2,2,
1.0, 0.0,
0.0, 1.0),
Vector_(2,
10.0, 5.0), 1)));
f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1", Matrix_(2,2,
-10.0, 0.0,
0.0, -10.0),
"x2", Matrix_(2,2,
10.0, 0.0,
0.0, 10.0),
Vector_(2,
1.0, -2.0), 1)));
f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x2", Matrix_(2,2,
-10.0, 0.0,
0.0, -10.0),
"x3", Matrix_(2,2,
10.0, 0.0,
0.0, 10.0),
Vector_(2,
1.5, -1.5), 1)));
f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x3", Matrix_(2,2,
-10.0, 0.0,
0.0, -10.0),
"x4", Matrix_(2,2,
10.0, 0.0,
0.0, 10.0),
Vector_(2,
2.0, -1.0), 1)));
f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1", I, Vector_(2,
10.0, 5.0), 1)));
f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1", -10 * I,
"x2", 10 * I, Vector_(2, 1.0, -2.0), 1)));
f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x2", -10 * I,
"x3", 10 * I, Vector_(2, 1.5, -1.5), 1)));
f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x3", -10 * I,
"x4", 10 * I, Vector_(2, 2.0, -1.0), 1)));
GaussianFactor combinedFactor(f);
@ -294,8 +274,9 @@ TEST( GaussianFactor, linearFactorN){
Vector b = Vector_(8,
10.0, 5.0, 1.0, -2.0, 1.5, -1.5, 2.0, -1.0);
GaussianFactor expected(combinedMeasurement, b, 1.);
CHECK(combinedFactor.equals(expected));
Vector sigmas = repeat(8,1.0);
GaussianFactor expected(combinedMeasurement, b, sigmas);
CHECK(assert_equal(expected,combinedFactor));
}
/* ************************************************************************* */
@ -336,45 +317,19 @@ TEST( GaussianFactor, eliminate )
boost::tie(actualCG,actualLF) = combined.eliminate("x2");
// create expected Conditional Gaussian
Matrix R11 = Matrix_(2,2,
1.0, 0.0,
0.0, 1.0
);
Matrix S12 = Matrix_(2,2,
-0.2, 0.0,
+0.0,-0.2
);
Matrix S13 = Matrix_(2,2,
-0.8, 0.0,
+0.0,-0.8
);
Matrix I = eye(2);
Matrix R11 = I, S12 = -0.2*I, S13 = -0.8*I;
Vector d(2); d(0) = 0.2; d(1) = -0.14;
Vector sigmas(2);
sigmas(0) = 1/sqrt(125.0);
sigmas(1) = 1/sqrt(125.0);
// Check the conditional Gaussian
GaussianConditional expectedCG("x2", d,R11,"l1",S12,"x1",S13,sigmas);
GaussianConditional
expectedCG("x2", d, R11, "l1", S12, "x1", S13, repeat(2, 1 / sqrt(125.0)));
// the expected linear factor
double sigma = 0.2236;
Matrix Bl1 = Matrix_(2,2,
// l1
1.00, 0.00,
0.00, 1.00
);
Matrix Bx1 = Matrix_(2,2,
// x1
-1.00, 0.00,
+0.00, -1.00
);
// the RHS
Matrix Bl1 = I, Bx1 = -I;
Vector b1(2); b1(0) = 0.0; b1(1) = 0.2;
GaussianFactor expectedLF("l1", Bl1, "x1", Bx1, b1, sigma);
GaussianFactor expectedLF("l1", Bl1, "x1", Bx1, b1, repeat(2,0.2236));
// check if the result matches
CHECK(assert_equal(expectedCG,*actualCG,1e-4));
@ -452,7 +407,7 @@ TEST( GaussianFactor, eliminate2 )
// the RHS
Vector b1(2); b1(0) = 0.0; b1(1) = 0.894427;
GaussianFactor expectedLF("l11", Bl1x1, b1*sigma, sigma);
GaussianFactor expectedLF("l11", Bl1x1, b1*sigma, repeat(2,sigma));
// check if the result matches
CHECK(assert_equal(expectedCG,*actualCG,1e-4));
@ -684,7 +639,7 @@ TEST( GaussianFactor, CONSTRUCTOR_GaussianConditional )
GaussianConditional::shared_ptr CG(new GaussianConditional("x2",d,R11,"l11",S12,sigmas));
GaussianFactor actualLF(CG);
// actualLF.print();
GaussianFactor expectedLF("x2",R11,"l11",S12,d, sigmas(0));
GaussianFactor expectedLF("x2",R11,"l11",S12,d, sigmas);
CHECK(assert_equal(expectedLF,actualLF,1e-5));
}

17
cpp/testNoiseModel.cpp
View File

@ -173,6 +173,23 @@ TEST( NoiseModel, QR )
CHECK(assert_equal(expectedRd2,Ab2,1e-6)); // Ab was modified in place !!!
}
/* ************************************************************************* */
TEST(NoiseModel, SmartCovariance )
{
bool smart = true;
sharedGaussian expected = Unit::Create(3);
sharedGaussian actual = Gaussian::Covariance(eye(3), smart);
CHECK(assert_equal(*expected,*actual));
}
/* ************************************************************************* */
TEST(NoiseModel, ScalarOrVector )
{
sharedGaussian expected = Unit::Create(3);
sharedGaussian actual = Gaussian::Covariance(eye(3), smart);
CHECK(assert_equal(*expected,*actual));
}
/* ************************************************************************* */
int main() {
TestResult tr;