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

Improved weighted eliminate to handle arbitrary linear equality constraints

release/4.3a0
Alex Cunningham 2009-11-11 14:42:09 +00:00
parent 2d4524374b
commit f51614813e
8 changed files with 246 additions and 133 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
cpp/LinearFactor.cpp
View File

@ -165,7 +165,7 @@ void LinearFactor::tally_separator(const string& key, set<string>& separator) co
}
/* ************************************************************************* */
pair<Matrix,Vector> LinearFactor::matrix(const Ordering& ordering) const {
pair<Matrix,Vector> LinearFactor::matrix(const Ordering& ordering, bool weight) const {
// get pointers to the matrices
vector<const Matrix *> matrices;
@ -174,12 +174,17 @@ pair<Matrix,Vector> LinearFactor::matrix(const Ordering& ordering) const {
matrices.push_back(&Aj);
}
// divide in sigma so error is indeed 0.5*|Ax-b|
Vector t = ediv(ones(sigmas_.size()),sigmas_);
Matrix A = vector_scale(collect(matrices), t);
// assemble
Matrix A = collect(matrices);
Vector b(b_);
// divide in sigma so error is indeed 0.5*|Ax-b|
if (weight) {
Vector t = ediv(ones(sigmas_.size()),sigmas_);
A = vector_scale(A, t);
for (int i=0; i<b_.size(); ++i)
b(i) *= t(i);
}
return make_pair(A, b);
}
@ -303,38 +308,41 @@ LinearFactor::eliminate(const string& key)
if (k != key) ordering += k;
// extract A, b from the combined linear factor (ensure that x is leading)
// use an augmented system [A b] to prevent copying
Matrix Ab = matrix_augmented(ordering);
size_t m = Ab.size1(); size_t n = Ab.size2()-1;
Matrix A; Vector b;
boost::tie(A, b) = matrix(ordering, false);
size_t n = A.size2();
// Do in-place QR to get R, d of the augmented system
if (verbose) ::print(A,"A before");
if (verbose) ::print(b,"b before");
std::list<boost::tuple<Vector, double, double> > solution =
weighted_eliminate(A, b, sigmas_);
// get dimensions of the eliminated variable
size_t n1 = getDim(key);
// if m<n1, this factor cannot be eliminated
size_t maxRank = min(m,n);
size_t maxRank = solution.size();
if (maxRank<n1)
throw(domain_error("LinearFactor::eliminate: fewer constraints than unknowns"));
// Do in-place QR to get R, d of the augmented system
if (verbose) ::print(Ab,"Rd before");
Matrix Rd; Vector sigmas;
boost::tie(Rd, sigmas) = weighted_eliminate(Ab, sigmas_);
if (verbose) ::print(Rd,"Rd after");
//NOTE: this actually normalizes the entire R matrix automatically,
// and produces the precisions
// This could probably be made more efficient, but is accurate
// extract RHS
Vector d(maxRank);
for (int i=0; i<maxRank; ++i)
d(i) = Rd(i,n);
// unpack the solutions
Matrix R(maxRank, n);
Vector r, d(maxRank), newSigmas(maxRank); double di, sigma;
Matrix::iterator2 Rit = R.begin2();
size_t i = 0;
BOOST_FOREACH(boost::tie(r, di, sigma), solution) {
copy(r.begin(), r.end(), Rit); // copy r vector
d(i) = di; // copy in rhs
newSigmas(i) = sigma; // copy in new sigmas
Rit += n; i += 1;
}
// create base conditional Gaussian
ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(key,
sub(d, 0, n1), // form d vector
sub(Rd, 0, n1, 0, n1), // form R matrix
sub(sigmas, 0, n1))); // get standard deviations
sub(R, 0, n1, 0, n1), // form R matrix
sub(newSigmas, 0, n1))); // get standard deviations
// extract the block matrices for parents in both CG and LF
LinearFactor::shared_ptr lf(new LinearFactor);
@ -342,13 +350,13 @@ LinearFactor::eliminate(const string& key)
BOOST_FOREACH(string cur_key, ordering)
if (cur_key!=key) {
size_t dim = getDim(cur_key);
cg->add(cur_key, sub(Rd, 0, n1, j, j+dim));
lf->insert(cur_key, sub(Rd, n1, maxRank, j, j+dim));
cg->add(cur_key, sub(R, 0, n1, j, j+dim));
lf->insert(cur_key, sub(R, n1, maxRank, j, j+dim));
j+=dim;
}
// Set sigmas
lf->sigmas_ = sub(sigmas,n1,maxRank);
lf->sigmas_ = sub(newSigmas,n1,maxRank);
// extract ds vector for the new b
lf->set_b(sub(d, n1, maxRank));

4
cpp/LinearFactor.h
View File

@ -185,10 +185,10 @@ public:
/**
* Return (dense) matrix associated with factor
* NOTE: in this case, the standard deviations are baked into A and b
* @param ordering of variables needed for matrix column order
* @param set weight to true to bake in the weights
*/
std::pair<Matrix, Vector> matrix(const Ordering& ordering) const;
std::pair<Matrix, Vector> matrix(const Ordering& ordering, bool weight = true) const;
/**
* Return (dense) matrix associated with factor

61
cpp/Matrix.cpp
View File

@ -7,6 +7,7 @@
#include <stdarg.h>
#include <string.h>
#include <iomanip>
#include <list>
#include <boost/foreach.hpp>
#include <boost/numeric/ublas/lu.hpp>
@ -160,6 +161,18 @@ Vector column(const Matrix& A, size_t j) {
return a;
}
/* ************************************************************************* */
Vector row(const Matrix& A, size_t i) {
if (i>=A.size1())
throw invalid_argument("Row index out of bounds!");
size_t n = A.size2();
Vector a(n);
for (int j=0; j<n; ++j)
a(j) = A(i,j);
return a;
}
/* ************************************************************************* */
void print(const Matrix& A, const string &s) {
size_t m = A.size1(), n = A.size2();
@ -285,18 +298,16 @@ void householder_update(Matrix &A, int j, double beta, const Vector& vjm) {
}
/* ************************************************************************* */
std::pair<Matrix, Vector> weighted_eliminate(Matrix& A, const Vector& sigmas) {
list<boost::tuple<Vector, double, double> >
weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) {
bool verbose = false;
// get sizes
size_t m = A.size1();
size_t n = A.size2();
size_t maxRank = min(m,n);
// create an R matrix to store the solution
Matrix R = eye(maxRank, n);
// create a sigma vector
Vector newSigmas(maxRank);
// create list
list<boost::tuple<Vector, double, double> > results;
// loop over the columns
for (int j=0; j<maxRank; ++j) {
@ -308,26 +319,40 @@ std::pair<Matrix, Vector> weighted_eliminate(Matrix& A, const Vector& sigmas) {
Vector pseudo; double precision;
if (verbose) print(sigmas, "sigmas");
boost::tie(pseudo, precision) = weightedPseudoinverse(a, sigmas);
// if precision is zero, rank(A) was less than maxRank, so return
if (precision < 1e-8) {
maxRank = j;
return results;
}
if (verbose) print(pseudo, "pseudo");
// create solution and copy into R
for (int j2=j; j2<n; ++j2) {
R(j,j2) = inner_prod(pseudo, column(A, j2));
}
if (verbose) print(R, "updatedR");
// create solution and copy into r
Vector r = basis(n, j);
for (int j2=j+1; j2<n; ++j2)
r(j2) = inner_prod(pseudo, column(A, j2));
// update A
for (int i=0;i<m;++i) // update all rows
for (int j2=j+1;j2<n;++j2) { // limit to only columns in separator
A(i,j2) -= R(j,j2)*a(i);
// create the rhs
double d = inner_prod(pseudo, b);
if (verbose) print(r, "updatedR");
if (verbose) cout << "d = " << d << endl;
// construct solution (r, d, sigma)
results.push_back(boost::make_tuple(r, d, 1./sqrt(precision)));
// update A, b
for (int i=0;i<m;++i) { // update all rows
double ai = a(i);
b(i) -= ai*d;
for (int j2=j+1;j2<n;++j2) // limit to only columns in separator
A(i,j2) -= ai*r(j2);
}
if (verbose) print(A, "updatedA");
//if (verbose) print(sub(A, 0,m, j+1,n), "updatedA"); // bad index
// save precision information
newSigmas[j] = sqrt(1./precision);
}
return make_pair(R, newSigmas);
return results;
}
/* ************************************************************************* */

18
cpp/Matrix.h
View File

@ -10,7 +10,9 @@
#pragma once
#include <list>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/tuple/tuple.hpp>
#include "Vector.h"
/**
@ -118,6 +120,14 @@ Matrix sub(const Matrix& A, size_t i1, size_t i2, size_t j1, size_t j2);
*/
Vector column(const Matrix& A, size_t j);
/**
* extracts a row from a matrix
* @param matrix to extract row from
* @param index of the row
* @return the row in vector form
*/
Vector row(const Matrix& A, size_t j);
// left multiply with scalar
//inline Matrix operator*(double s, const Matrix& A) { return A*s;}
@ -148,11 +158,13 @@ void householder_update(Matrix &A, int j, double beta, const Vector& vjm);
/**
* Imperative algorithm for in-place full elimination with
* weights and constraint handling
* @param Ab is an augmented system to eliminate
* @param A is a matrix to eliminate
* @param b is the rhs
* @param sigmas is a vector of the measurement standard deviation
* @return a pair of R (normalized) and new sigmas
* @return list of r vectors, d and sigma
*/
std::pair<Matrix, Vector> weighted_eliminate(Matrix& Ab, const Vector& sigmas);
std::list<boost::tuple<Vector, double, double> >
weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas);
/**
* Householder tranformation, Householder vectors below diagonal

44
cpp/Vector.cpp
View File

@ -93,6 +93,13 @@ namespace gtsam {
return v;
}
/* ************************************************************************* */
Vector delta(size_t n, size_t i, double value) {
Vector v = zero(n);
v(i) = value;
return v;
}
/* ************************************************************************* */
void print(const Vector& v, const string& s) {
size_t n = v.size();
@ -182,46 +189,33 @@ namespace gtsam {
}
/* ************************************************************************* */
pair<Vector, double> weightedPseudoinverse(const Vector& v, const Vector& sigmas) {
if (v.size() != sigmas.size())
pair<Vector, double> weightedPseudoinverse(const Vector& a, const Vector& sigmas) {
size_t m = sigmas.size();
if (a.size() != m)
throw invalid_argument("V and precisions have different sizes!");
// detect constraints and sanity-check
int constraint_index = -1;
for(int i=0; i<sigmas.size(); ++i) {
if (sigmas[i] < 1e-9 && v[i] > 1e-9) {
if (constraint_index != -1)
throw invalid_argument("Multiple constraints on a single node!");
else
constraint_index = i;
}
}
for(int i=0; i<m; ++i)
if (sigmas[i] < 1e-9 && fabs(a[i]) > 1e-9)
return make_pair(delta(m,i,1/a[i]), 1.0/0.0);
// compute pseudoinverse
if (constraint_index != -1) {
// constrained case
Vector sol = zero(sigmas.size());
sol(constraint_index) = 1.0;
return make_pair(sol, 1.0/0.0);
} else {
// normal case
double normV = 0.;
Vector precisions(sigmas.size());
for(int i = 0; i<v.size(); i++) {
Vector precisions(m);
for(int i = 0; i<a.size(); i++) {
if (sigmas[i] < 1e-5) {
precisions[i] = 1./0.;
} else {
precisions[i] = 1./(sigmas[i]*sigmas[i]);
normV += v[i]*v[i]*precisions[i];
normV += a[i]*a[i]*precisions[i];
}
}
Vector sol = zero(v.size());
for(int i = 0; i<v.size(); i++)
Vector sol = zero(a.size());
for(int i = 0; i<a.size(); i++)
if (sigmas[i] > 1e-5)
sol[i] = precisions[i]*v[i];
sol[i] = precisions[i]*a[i];
return make_pair(sol/normV, normV);
}
}
/* ************************************************************************* */
Vector concatVectors(size_t nrVectors, ...)

19
cpp/Vector.h
View File

@ -41,6 +41,25 @@ Vector Vector_(size_t m, ...);
*/
Vector repeat(size_t n, double value);
/**
* Create basis vector of dimension n,
* with a constant in spot i
* @param n is the size of the vector
* @param index of the one
* @param value is the value to insert into the vector
* @return delta vector
*/
Vector delta(size_t n, size_t i, double value);
/**
* Create basis vector of dimension n,
* with one in spot i
* @param n is the size of the vector
* @param index of the one
* @return basis vector
*/
inline Vector basis(size_t n, size_t i) { return delta(n, i, 1.0); }
/**
* Create zero vector
* @param size

78
cpp/testLinearFactorGraph.cpp
View File

@ -606,37 +606,53 @@ TEST( LinearFactorGraph, constrained_simple )
}
// These tests require multiple constraints on a single node and will fail
///* ************************************************************************* */
//TEST( LinearFactorGraph, constrained_single )
//{
// // get a graph with a constraint in it
// LinearFactorGraph fg = createSingleConstraintGraph();
//
// // eliminate and solve
// Ordering ord;
// ord += "x", "y";
// VectorConfig actual = fg.optimize(ord);
//
// // verify
// VectorConfig expected = createSingleConstraintConfig();
// CHECK(assert_equal(actual, expected));
//}
//
///* ************************************************************************* */
//TEST( LinearFactorGraph, constrained_multi )
//{
// // get a graph with a constraint in it
// LinearFactorGraph fg = createMultiConstraintGraph();
//
// // eliminate and solve
// Ordering ord;
// ord += "x", "y", "z";
// VectorConfig actual = fg.optimize(ord);
//
// // verify
// VectorConfig expected = createMultiConstraintConfig();
// CHECK(assert_equal(actual, expected));
//}
/* ************************************************************************* */
TEST( LinearFactorGraph, constrained_single )
{
// get a graph with a constraint in it
LinearFactorGraph fg = createSingleConstraintGraph();
// eliminate and solve
Ordering ord;
ord += "x", "y";
VectorConfig actual = fg.optimize(ord);
// verify
VectorConfig expected = createSingleConstraintConfig();
CHECK(assert_equal(actual, expected));
}
/* ************************************************************************* */
TEST( LinearFactorGraph, constrained_single2 )
{
// get a graph with a constraint in it
LinearFactorGraph fg = createSingleConstraintGraph();
// eliminate and solve
Ordering ord;
ord += "y", "x";
VectorConfig actual = fg.optimize(ord);
// verify
VectorConfig expected = createSingleConstraintConfig();
CHECK(assert_equal(actual, expected));
}
/* ************************************************************************* */
TEST( LinearFactorGraph, constrained_multi )
{
// get a graph with a constraint in it
LinearFactorGraph fg = createMultiConstraintGraph();
// eliminate and solve
Ordering ord;
ord += "x", "y", "z";
VectorConfig actual = fg.optimize(ord);
// verify
VectorConfig expected = createMultiConstraintConfig();
CHECK(assert_equal(actual, expected));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}

69
cpp/testMatrix.cpp
View File

@ -8,6 +8,7 @@
#include <iostream>
#include <CppUnitLite/TestHarness.h>
#include <boost/tuple/tuple.hpp>
#include <boost/foreach.hpp>
#include "Matrix.h"
using namespace std;
@ -123,6 +124,27 @@ TEST( matrix, column )
CHECK(assert_equal(a3, exp3));
}
/* ************************************************************************* */
TEST( matrix, row )
{
Matrix A = Matrix_(4, 7,
-1., 0., 1., 0., 0., 0., -0.2,
0., -1., 0., 1., 0., 0., 0.3,
1., 0., 0., 0., -1., 0., 0.2,
0., 1., 0., 0., 0., -1., -0.1);
Vector a1 = row(A, 0);
Vector exp1 = Vector_(7, -1., 0., 1., 0., 0., 0., -0.2);
CHECK(assert_equal(a1, exp1));
Vector a2 = row(A, 2);
Vector exp2 = Vector_(7, 1., 0., 0., 0., -1., 0., 0.2);
CHECK(assert_equal(a2, exp2));
Vector a3 = row(A, 3);
Vector exp3 = Vector_(7, 0., 1., 0., 0., 0., -1., -0.1);
CHECK(assert_equal(a3, exp3));
}
/* ************************************************************************* */
TEST( matrix, zeros )
{
@ -532,26 +554,43 @@ TEST( matrix, svd )
/* ************************************************************************* */
TEST( matrix, weighted_elimination )
{
// create a matrix to eliminate - assume augmented
Matrix A = Matrix_(4, 7,
-1., 0., 1., 0., 0., 0., -0.2,
0., -1., 0., 1., 0., 0., 0.3,
1., 0., 0., 0., -1., 0., 0.2,
0., 1., 0., 0., 0., -1., -0.1);
// create a matrix to eliminate
Matrix A = Matrix_(4, 6,
-1., 0., 1., 0., 0., 0.,
0., -1., 0., 1., 0., 0.,
1., 0., 0., 0., -1., 0.,
0., 1., 0., 0., 0., -1.);
Vector b = Vector_(4, -0.2, 0.3, 0.2, -0.1);
Vector sigmas = Vector_(4, 0.2, 0.2, 0.1, 0.1);
// perform elimination
Matrix actual; Vector newSigmas;
boost::tie(actual, newSigmas) = weighted_eliminate(A, sigmas);
std::list<boost::tuple<Vector, double, double> > solution =
weighted_eliminate(A, b, sigmas);
// verify
Matrix expected = Matrix_(4, 7,
1., 0.,-0.2, 0.,-0.8, 0., 0.2,
0., 1., 0.,-0.2, 0.,-0.8,-0.14,
0., 0., 1., 0., -1., 0., 0.0,
0., 0., 0., 1., 0., -1., 0.2);
CHECK(assert_equal(actual,expected));
// expected values
Matrix expectedR = Matrix_(4, 6,
1., 0.,-0.2, 0.,-0.8, 0.,
0., 1., 0.,-0.2, 0.,-0.8,
0., 0., 1., 0., -1., 0.,
0., 0., 0., 1., 0., -1.);
Vector d = Vector_(4, 0.2, -0.14, 0.0, 0.2);
Vector newSigmas = Vector_(4,
0.0894427,
0.0894427,
0.223607,
0.223607);
// unpack and verify
Vector r; double di, sigma;
size_t i = 0;
BOOST_FOREACH(boost::tie(r, di, sigma), solution) {
CHECK(assert_equal(r, row(expectedR, i))); // verify r
DOUBLES_EQUAL(d(i), di, 1e-8); // verify d
DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5); // verify sigma
i += 1;
}
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }