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

Build a dual graph to compute dual values for equality constrained factors

release/4.3a0
thduynguyen 2014-09-13 01:36:04 -04:00
parent 4ca9d5757f
commit 20fb8ab77d
4 changed files with 106 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -391,6 +391,46 @@ namespace gtsam {
return false;
}
/* ************************************************************************* */
JacobianFactor::shared_ptr GaussianFactorGraph::createDualFactor(Key key,
const VariableIndex& variableIndex, const VectorValues& delta) const {
typedef GaussianFactor G;
typedef JacobianFactor J;
std::vector<std::pair<Key, Matrix> > Aterms;
Vector b = zero(delta.at(key).size());
BOOST_FOREACH(size_t factorIx, variableIndex[key]) {
// If it is a constraint, transpose the A matrix to have the jacobian of the dual key
J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(this->at(factorIx)));
if (jacobian && jacobian->get_model() && jacobian->get_model()->isConstrained()) {
Matrix Ai = jacobian->getA(jacobian->find(key)).transpose();
boost::optional<Key> dualKey = jacobian->dualKey();
if (!dualKey) {
throw std::runtime_error("Fail to create dual factor! " \
"Constrained JacobianFactor doesn't have a dual key!");
}
Aterms.push_back(make_pair(*(dualKey), Ai));
}
else {
// If it is unconstrained, collect the gradient to the b vector
G::shared_ptr factor(boost::dynamic_pointer_cast<G>(this->at(factorIx)));
b += factor->gradient(key, delta);
}
}
return boost::make_shared<JacobianFactor>(Aterms, b);
}
/* ************************************************************************* */
GaussianFactorGraph::shared_ptr GaussianFactorGraph::buildDualGraph(
const KeySet& constrainedVariables,
const VariableIndex& variableIndex,
const VectorValues& delta) const {
GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph());
BOOST_FOREACH(const Key key, constrainedVariables) {
dualGraph->push_back(createDualFactor(key, variableIndex, delta));
}
return dualGraph;
}
/* ************************************************************************* */
boost::tuple<GaussianFactorGraph, GaussianFactorGraph, GaussianFactorGraph> GaussianFactorGraph::splitConstraints() const {
typedef HessianFactor H;

12
gtsam/linear/GaussianFactorGraph.h
View File

@ -328,6 +328,18 @@ namespace gtsam {
*/
boost::tuple<GaussianFactorGraph, GaussianFactorGraph, GaussianFactorGraph> splitConstraints() const;
/**
* Build a dual graph to estimate dual variables associated with constrained factors
*/
GaussianFactorGraph::shared_ptr buildDualGraph(
const KeySet& constrainedVariables, const VariableIndex& variableIndex,
const VectorValues& delta) const;
/**
* Create a dual factor from a constrained key
*/
JacobianFactor::shared_ptr createDualFactor(Key key,
const VariableIndex& variableIndex, const VectorValues& delta) const;
private:
/** Serialization function */

10
tests/smallExample.h
View File

@ -166,7 +166,7 @@ static SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2);
static SharedDiagonal constraintModel = noiseModel::Constrained::All(2);
static const Key _l1_=0, _x1_=1, _x2_=2;
static const Key _x_=0, _y_=1, _z_=2;
static const Key _x_=0, _y_=1, _z_=2, _d_=3, _e_=4;
} // \namespace impl
@ -423,7 +423,7 @@ inline GaussianFactorGraph createSimpleConstraintGraph() {
Matrix Ay1 = eye(2) * -1;
Vector b2 = (Vector(2) << 0.0, 0.0);
JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2,
constraintModel));
constraintModel, _d_));
// construct the graph
GaussianFactorGraph fg;
@ -469,7 +469,7 @@ inline GaussianFactorGraph createSingleConstraintGraph() {
Matrix Ay1 = eye(2) * 10;
Vector b2 = (Vector(2) << 1.0, 2.0);
JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2,
constraintModel));
constraintModel, _d_));
// construct the graph
GaussianFactorGraph fg;
@ -513,7 +513,7 @@ inline GaussianFactorGraph createMultiConstraintGraph() {
b1(0) = 1.0;
b1(1) = 2.0;
JacobianFactor::shared_ptr lc1(new JacobianFactor(_x_, A11, _y_, A12, b1,
constraintModel));
constraintModel, _d_));
// constraint 2
Matrix A21(2, 2);
@ -532,7 +532,7 @@ inline GaussianFactorGraph createMultiConstraintGraph() {
b2(0) = 3.0;
b2(1) = 4.0;
JacobianFactor::shared_ptr lc2(new JacobianFactor(_x_, A21, _z_, A22, b2,
constraintModel));
constraintModel, _e_));
// construct the graph
GaussianFactorGraph fg;

49
tests/testGaussianFactorGraphB.cpp
View File

@ -593,6 +593,55 @@ TEST( GaussianFactorGraph, conditional_sigma_failure) {
}
}
/* ************************************************************************* */
TEST( GaussianFactorGraph, buildDualGraph1 )
{
GaussianFactorGraph fgc1 = createSimpleConstraintGraph();
KeySet constrainedVariables1 = list_of(0)(1);
VariableIndex variableIndex1(fgc1);
VectorValues delta1 = createSimpleConstraintValues();
GaussianFactorGraph::shared_ptr dualGraph1 = fgc1.buildDualGraph(
constrainedVariables1, variableIndex1, delta1);
GaussianFactorGraph expectedDualGraph1;
expectedDualGraph1.push_back(JacobianFactor(3, eye(2), zero(2)));
expectedDualGraph1.push_back(JacobianFactor(3, -eye(2), zero(2)));
EXPECT(assert_equal(expectedDualGraph1, *dualGraph1));
}
/* ************************************************************************* */
TEST( GaussianFactorGraph, buildDualGraph2 )
{
GaussianFactorGraph fgc2 = createSingleConstraintGraph();
KeySet constrainedVariables2 = list_of(0)(1);
VariableIndex variableIndex2(fgc2);
VectorValues delta2 = createSingleConstraintValues();
GaussianFactorGraph::shared_ptr dualGraph2 = fgc2.buildDualGraph(
constrainedVariables2, variableIndex2, delta2);
GaussianFactorGraph expectedDualGraph2;
expectedDualGraph2.push_back(JacobianFactor(3, (Matrix(2,2) << 1,2,2,1), zero(2)));
expectedDualGraph2.push_back(JacobianFactor(3, 10*eye(2), zero(2)));
EXPECT(assert_equal(expectedDualGraph2, *dualGraph2));
}
/* ************************************************************************* */
TEST( GaussianFactorGraph, buildDualGraph3 )
{
GaussianFactorGraph fgc3 = createMultiConstraintGraph();
KeySet constrainedVariables3 = list_of(0)(1)(2);
VariableIndex variableIndex3(fgc3);
VectorValues delta3 = createMultiConstraintValues();
GaussianFactorGraph::shared_ptr dualGraph3 = fgc3.buildDualGraph(
constrainedVariables3, variableIndex3, delta3);
GaussianFactorGraph expectedDualGraph3;
expectedDualGraph3.push_back(
JacobianFactor(3, (Matrix(2, 2) << 1, 2, 2, 1), 4,
(Matrix(2, 2) << 3, -1, 4, -2), zero(2)));
expectedDualGraph3.push_back(JacobianFactor(3, 10*eye(2), zero(2)));
expectedDualGraph3.push_back(
JacobianFactor(4, (Matrix(2, 2) << 1, 1, 1, 2), zero(2)));
EXPECT(assert_equal(expectedDualGraph3, *dualGraph3));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */