Added and tested whitening parameter for matrix() and matrix_augmented()
parent
fbee0bd749
commit
fb0ca07bf1
|
@ -215,7 +215,7 @@ pair<Matrix,Vector> GaussianFactor::matrix(const Ordering& ordering, bool weight
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
Matrix GaussianFactor::matrix_augmented(const Ordering& ordering) const {
|
Matrix GaussianFactor::matrix_augmented(const Ordering& ordering, bool weight) const {
|
||||||
// get pointers to the matrices
|
// get pointers to the matrices
|
||||||
vector<const Matrix *> matrices;
|
vector<const Matrix *> matrices;
|
||||||
BOOST_FOREACH(const Symbol& j, ordering) {
|
BOOST_FOREACH(const Symbol& j, ordering) {
|
||||||
|
@ -229,7 +229,11 @@ Matrix GaussianFactor::matrix_augmented(const Ordering& ordering) const {
|
||||||
B_mat(i,0) = b_(i);
|
B_mat(i,0) = b_(i);
|
||||||
matrices.push_back(&B_mat);
|
matrices.push_back(&B_mat);
|
||||||
|
|
||||||
return collect(matrices);
|
// divide in sigma so error is indeed 0.5*|Ax-b|
|
||||||
|
Matrix Ab = collect(matrices);
|
||||||
|
if (weight) model_->WhitenInPlace(Ab);
|
||||||
|
|
||||||
|
return Ab;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
@ -233,10 +233,10 @@ public:
|
||||||
/**
|
/**
|
||||||
* Return (dense) matrix associated with factor
|
* Return (dense) matrix associated with factor
|
||||||
* The returned system is an augmented matrix: [A b]
|
* The returned system is an augmented matrix: [A b]
|
||||||
* The standard deviations are NOT baked into A and b
|
|
||||||
* @param ordering of variables needed for matrix column order
|
* @param ordering of variables needed for matrix column order
|
||||||
|
* @param set weight to use whitening to bake in weights
|
||||||
*/
|
*/
|
||||||
Matrix matrix_augmented(const Ordering& ordering) const;
|
Matrix matrix_augmented(const Ordering& ordering, bool weight = true) const;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Return vectors i, j, and s to generate an m-by-n sparse matrix
|
* Return vectors i, j, and s to generate an m-by-n sparse matrix
|
||||||
|
|
|
@ -25,6 +25,7 @@ using namespace boost::assign;
|
||||||
using namespace std;
|
using namespace std;
|
||||||
using namespace gtsam;
|
using namespace gtsam;
|
||||||
using namespace example;
|
using namespace example;
|
||||||
|
using namespace boost;
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
TEST( GaussianFactor, linearFactor )
|
TEST( GaussianFactor, linearFactor )
|
||||||
|
@ -510,16 +511,36 @@ TEST( GaussianFactor, matrix )
|
||||||
Ordering ord;
|
Ordering ord;
|
||||||
ord += "x1","x2";
|
ord += "x1","x2";
|
||||||
|
|
||||||
Matrix A; Vector b;
|
// Test whitened version
|
||||||
boost::tie(A,b) = lf->matrix(ord);
|
Matrix A_act1; Vector b_act1;
|
||||||
|
boost::tie(A_act1,b_act1) = lf->matrix(ord, true);
|
||||||
|
|
||||||
Matrix A1 = Matrix_(2,4,
|
Matrix A1 = Matrix_(2,4,
|
||||||
-10.0, 0.0, 10.0, 0.0,
|
-10.0, 0.0, 10.0, 0.0,
|
||||||
000.0,-10.0, 0.0, 10.0 );
|
000.0,-10.0, 0.0, 10.0 );
|
||||||
Vector b1 = Vector_(2, 2.0, -1.0);
|
Vector b1 = Vector_(2, 2.0, -1.0);
|
||||||
|
|
||||||
EQUALITY(A,A1);
|
EQUALITY(A_act1,A1);
|
||||||
EQUALITY(b,b1);
|
EQUALITY(b_act1,b1);
|
||||||
|
|
||||||
|
// Test unwhitened version
|
||||||
|
Matrix A_act2; Vector b_act2;
|
||||||
|
boost::tie(A_act2,b_act2) = lf->matrix(ord, false);
|
||||||
|
|
||||||
|
|
||||||
|
Matrix A2 = Matrix_(2,4,
|
||||||
|
-1.0, 0.0, 1.0, 0.0,
|
||||||
|
000.0,-1.0, 0.0, 1.0 );
|
||||||
|
Vector b2 = Vector_(2, 0.2, -0.1);
|
||||||
|
|
||||||
|
EQUALITY(A_act2,A2);
|
||||||
|
EQUALITY(b_act2,b2);
|
||||||
|
|
||||||
|
// Ensure that whitening is consistent
|
||||||
|
shared_ptr<noiseModel::Gaussian> model = lf->get_model();
|
||||||
|
model->WhitenSystem(A_act2, b_act2);
|
||||||
|
EQUALITY(A_act1, A_act2);
|
||||||
|
EQUALITY(b_act1, b_act2);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
@ -535,14 +556,30 @@ TEST( GaussianFactor, matrix_aug )
|
||||||
Ordering ord;
|
Ordering ord;
|
||||||
ord += "x1","x2";
|
ord += "x1","x2";
|
||||||
|
|
||||||
Matrix Ab;
|
// Test unwhitened version
|
||||||
Ab = lf->matrix_augmented(ord);
|
Matrix Ab_act1;
|
||||||
|
Ab_act1 = lf->matrix_augmented(ord, false);
|
||||||
|
|
||||||
Matrix Ab1 = Matrix_(2,5,
|
Matrix Ab1 = Matrix_(2,5,
|
||||||
-1.0, 0.0, 1.0, 0.0, 0.2,
|
-1.0, 0.0, 1.0, 0.0, 0.2,
|
||||||
00.0,- 1.0, 0.0, 1.0, -0.1 );
|
00.0,- 1.0, 0.0, 1.0, -0.1 );
|
||||||
|
|
||||||
EQUALITY(Ab,Ab1);
|
EQUALITY(Ab_act1,Ab1);
|
||||||
|
|
||||||
|
// Test whitened version
|
||||||
|
Matrix Ab_act2;
|
||||||
|
Ab_act2 = lf->matrix_augmented(ord, true);
|
||||||
|
|
||||||
|
Matrix Ab2 = Matrix_(2,5,
|
||||||
|
-10.0, 0.0, 10.0, 0.0, 2.0,
|
||||||
|
00.0, -10.0, 0.0, 10.0, -1.0 );
|
||||||
|
|
||||||
|
EQUALITY(Ab_act2,Ab2);
|
||||||
|
|
||||||
|
// Ensure that whitening is consistent
|
||||||
|
shared_ptr<noiseModel::Gaussian> model = lf->get_model();
|
||||||
|
model->WhitenInPlace(Ab_act1);
|
||||||
|
EQUALITY(Ab_act1, Ab_act2);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
Loading…
Reference in New Issue