Added planar graph with easy subtree
Frank Dellaert
parent
07cc95e4c4
commit
730f4a546f
|
|
@ -31,7 +31,8 @@ typedef boost::shared_ptr<NonlinearFactor<VectorConfig> > shared;
|
|||
/* ************************************************************************* */
|
||||
boost::shared_ptr<const ExampleNonlinearFactorGraph> sharedNonlinearFactorGraph() {
|
||||
// Create
|
||||
boost::shared_ptr<ExampleNonlinearFactorGraph> nlfg(new ExampleNonlinearFactorGraph);
|
||||
boost::shared_ptr<ExampleNonlinearFactorGraph> nlfg(
|
||||
new ExampleNonlinearFactorGraph);
|
||||
|
||||
// prior on x1
|
||||
double sigma1 = 0.1;
|
||||
|
|
@ -41,19 +42,25 @@ boost::shared_ptr<const ExampleNonlinearFactorGraph> sharedNonlinearFactorGraph(
|
|||
|
||||
// odometry between x1 and x2
|
||||
double sigma2 = 0.1;
|
||||
Vector z2(2); z2(0) = 1.5 ; z2(1) = 0;
|
||||
Vector z2(2);
|
||||
z2(0) = 1.5;
|
||||
z2(1) = 0;
|
||||
shared f2(new Simulated2DOdometry(z2, sigma2, "x1", "x2"));
|
||||
nlfg->push_back(f2);
|
||||
|
||||
// measurement between x1 and l1
|
||||
double sigma3 = 0.2;
|
||||
Vector z3(2); z3(0) = 0. ; z3(1) = -1.;
|
||||
Vector z3(2);
|
||||
z3(0) = 0.;
|
||||
z3(1) = -1.;
|
||||
shared f3(new Simulated2DMeasurement(z3, sigma3, "x1", "l1"));
|
||||
nlfg->push_back(f3);
|
||||
|
||||
// measurement between x2 and l1
|
||||
double sigma4 = 0.2;
|
||||
Vector z4(2); z4(0)= -1.5 ; z4(1) = -1.;
|
||||
Vector z4(2);
|
||||
z4(0) = -1.5;
|
||||
z4(1) = -1.;
|
||||
shared f4(new Simulated2DMeasurement(z4, sigma4, "x2", "l1"));
|
||||
nlfg->push_back(f4);
|
||||
|
||||
|
|
@ -84,7 +91,9 @@ boost::shared_ptr<const VectorConfig> sharedNoisyConfig() {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorConfig createNoisyConfig() { return *sharedNoisyConfig();}
|
||||
VectorConfig createNoisyConfig() {
|
||||
return *sharedNoisyConfig();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorConfig createCorrectDelta() {
|
||||
|
|
@ -105,8 +114,7 @@ VectorConfig createZeroDelta() {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph createGaussianFactorGraph()
|
||||
{
|
||||
GaussianFactorGraph createGaussianFactorGraph() {
|
||||
Matrix I = eye(2);
|
||||
VectorConfig c = createNoisyConfig();
|
||||
|
||||
|
|
@ -142,18 +150,18 @@ GaussianFactorGraph createGaussianFactorGraph()
|
|||
* 1 1 9
|
||||
* 1 5
|
||||
*/
|
||||
GaussianBayesNet createSmallGaussianBayesNet()
|
||||
{
|
||||
GaussianBayesNet createSmallGaussianBayesNet() {
|
||||
Matrix R11 = Matrix_(1, 1, 1.0), S12 = Matrix_(1, 1, 1.0);
|
||||
Matrix R22 = Matrix_(1, 1, 1.0);
|
||||
Vector d1(1), d2(1);
|
||||
d1(0) = 9; d2(0) = 5;
|
||||
Vector tau(1); tau(0) = 1.0;
|
||||
d1(0) = 9;
|
||||
d2(0) = 5;
|
||||
Vector tau(1);
|
||||
tau(0) = 1.0;
|
||||
|
||||
// define nodes and specify in reverse topological sort (i.e. parents last)
|
||||
GaussianConditional::shared_ptr
|
||||
Px_y(new GaussianConditional("x",d1,R11,"y",S12,tau)),
|
||||
Py(new GaussianConditional("y",d2,R22,tau));
|
||||
GaussianConditional::shared_ptr Px_y(new GaussianConditional("x", d1, R11,
|
||||
"y", S12, tau)), Py(new GaussianConditional("y", d2, R22, tau));
|
||||
GaussianBayesNet cbn;
|
||||
cbn.push_back(Px_y);
|
||||
cbn.push_back(Py);
|
||||
|
|
@ -168,21 +176,23 @@ namespace smallOptimize {
|
|||
Vector h(const Vector& v) {
|
||||
double x = v(0);
|
||||
return Vector_(2, cos(x), sin(x));
|
||||
};
|
||||
}
|
||||
;
|
||||
Matrix H(const Vector& v) {
|
||||
double x = v(0);
|
||||
return Matrix_(2, 1, -sin(x), cos(x));
|
||||
};
|
||||
}
|
||||
;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
boost::shared_ptr<const ExampleNonlinearFactorGraph> sharedReallyNonlinearFactorGraph()
|
||||
{
|
||||
boost::shared_ptr<ExampleNonlinearFactorGraph> fg(new ExampleNonlinearFactorGraph);
|
||||
boost::shared_ptr<const ExampleNonlinearFactorGraph> sharedReallyNonlinearFactorGraph() {
|
||||
boost::shared_ptr<ExampleNonlinearFactorGraph> fg(
|
||||
new ExampleNonlinearFactorGraph);
|
||||
Vector z = Vector_(2, 1.0, 0.0);
|
||||
double sigma = 0.1;
|
||||
boost::shared_ptr<NonlinearFactor1>
|
||||
factor(new NonlinearFactor1(z,sigma,&smallOptimize::h,"x",&smallOptimize::H));
|
||||
boost::shared_ptr<NonlinearFactor1> factor(new NonlinearFactor1(z, sigma,
|
||||
&smallOptimize::h, "x", &smallOptimize::H));
|
||||
fg->push_back(factor);
|
||||
return fg;
|
||||
}
|
||||
|
|
@ -212,7 +222,8 @@ pair<ExampleNonlinearFactorGraph, VectorConfig> createNonlinearSmoother(int T) {
|
|||
// odometry between x_t and x_{t-1}
|
||||
Vector odo = Vector_(2, 1.0, 0.0);
|
||||
string key = symbol('x', t);
|
||||
shared odometry(new Simulated2DOdometry(odo, sigma2, symbol('x', t - 1), key));
|
||||
shared odometry(new Simulated2DOdometry(odo, sigma2, symbol('x', t - 1),
|
||||
key));
|
||||
nlfg.push_back(odometry);
|
||||
|
||||
// measurement on x_t is like perfect GPS
|
||||
|
|
@ -255,8 +266,8 @@ GaussianFactorGraph createSimpleConstraintGraph() {
|
|||
Matrix Ax1 = eye(2);
|
||||
Matrix Ay1 = eye(2) * -1;
|
||||
Vector b2 = Vector_(2, 0.0, 0.0);
|
||||
GaussianFactor::shared_ptr f2(
|
||||
new GaussianFactor("x", Ax1, "y", Ay1, b2, 0.0));
|
||||
GaussianFactor::shared_ptr f2(new GaussianFactor("x", Ax1, "y", Ay1, b2,
|
||||
0.0));
|
||||
|
||||
// construct the graph
|
||||
GaussianFactorGraph fg;
|
||||
|
|
@ -291,12 +302,14 @@ GaussianFactorGraph createSingleConstraintGraph() {
|
|||
// |1 2||x_1| + |10 0||y_1| = |1|
|
||||
// |2 1||x_2| |0 10||y_2| |2|
|
||||
Matrix Ax1(2, 2);
|
||||
Ax1(0, 0) = 1.0; Ax1(0, 1) = 2.0;
|
||||
Ax1(1, 0) = 2.0; Ax1(1, 1) = 1.0;
|
||||
Ax1(0, 0) = 1.0;
|
||||
Ax1(0, 1) = 2.0;
|
||||
Ax1(1, 0) = 2.0;
|
||||
Ax1(1, 1) = 1.0;
|
||||
Matrix Ay1 = eye(2) * 10;
|
||||
Vector b2 = Vector_(2, 1.0, 2.0);
|
||||
GaussianFactor::shared_ptr f2(
|
||||
new GaussianFactor("x", Ax1, "y", Ay1, b2, 0.0));
|
||||
GaussianFactor::shared_ptr f2(new GaussianFactor("x", Ax1, "y", Ay1, b2,
|
||||
0.0));
|
||||
|
||||
// construct the graph
|
||||
GaussianFactorGraph fg;
|
||||
|
|
@ -324,29 +337,41 @@ GaussianFactorGraph createMultiConstraintGraph() {
|
|||
|
||||
// constraint 1
|
||||
Matrix A11(2, 2);
|
||||
A11(0,0) = 1.0 ; A11(0,1) = 2.0;
|
||||
A11(1,0) = 2.0 ; A11(1,1) = 1.0;
|
||||
A11(0, 0) = 1.0;
|
||||
A11(0, 1) = 2.0;
|
||||
A11(1, 0) = 2.0;
|
||||
A11(1, 1) = 1.0;
|
||||
|
||||
Matrix A12(2, 2);
|
||||
A12(0,0) = 10.0 ; A12(0,1) = 0.0;
|
||||
A12(1,0) = 0.0 ; A12(1,1) = 10.0;
|
||||
A12(0, 0) = 10.0;
|
||||
A12(0, 1) = 0.0;
|
||||
A12(1, 0) = 0.0;
|
||||
A12(1, 1) = 10.0;
|
||||
|
||||
Vector b1(2);
|
||||
b1(0) = 1.0; b1(1) = 2.0;
|
||||
GaussianFactor::shared_ptr lc1(new GaussianFactor("x", A11, "y", A12, b1, 0.0));
|
||||
b1(0) = 1.0;
|
||||
b1(1) = 2.0;
|
||||
GaussianFactor::shared_ptr lc1(new GaussianFactor("x", A11, "y", A12, b1,
|
||||
0.0));
|
||||
|
||||
// constraint 2
|
||||
Matrix A21(2, 2);
|
||||
A21(0,0) = 3.0 ; A21(0,1) = 4.0;
|
||||
A21(1,0) = -1.0 ; A21(1,1) = -2.0;
|
||||
A21(0, 0) = 3.0;
|
||||
A21(0, 1) = 4.0;
|
||||
A21(1, 0) = -1.0;
|
||||
A21(1, 1) = -2.0;
|
||||
|
||||
Matrix A22(2, 2);
|
||||
A22(0,0) = 1.0 ; A22(0,1) = 1.0;
|
||||
A22(1,0) = 1.0 ; A22(1,1) = 2.0;
|
||||
A22(0, 0) = 1.0;
|
||||
A22(0, 1) = 1.0;
|
||||
A22(1, 0) = 1.0;
|
||||
A22(1, 1) = 2.0;
|
||||
|
||||
Vector b2(2);
|
||||
b2(0) = 3.0; b2(1) = 4.0;
|
||||
GaussianFactor::shared_ptr lc2(new GaussianFactor("x", A21, "z", A22, b2, 0.0));
|
||||
b2(0) = 3.0;
|
||||
b2(1) = 4.0;
|
||||
GaussianFactor::shared_ptr lc2(new GaussianFactor("x", A21, "z", A22, b2,
|
||||
0.0));
|
||||
|
||||
// construct the graph
|
||||
GaussianFactorGraph fg;
|
||||
|
|
@ -468,4 +493,91 @@ VectorConfig createMultiConstraintConfig() {
|
|||
// return cbn;
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Create key for simulated planar graph
|
||||
string key(int x, int y) {
|
||||
stringstream ss;
|
||||
ss << "x" << x << y;
|
||||
return ss.str();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<GaussianFactorGraph, VectorConfig> planarGraph(size_t N) {
|
||||
|
||||
// create empty graph
|
||||
NonlinearFactorGraph<VectorConfig> nlfg;
|
||||
|
||||
// Create almost hard constraint on x11, sigma=0 will work for PCG not for normal
|
||||
double sigma0 = 1e-3;
|
||||
shared constraint(new Point2Prior(Vector_(2, 1.0, 1.0), sigma0, "x11"));
|
||||
nlfg.push_back(constraint);
|
||||
|
||||
double sigma = 0.01;
|
||||
|
||||
// Create horizontal constraints, 1...N*(N-1)
|
||||
Vector z1 = Vector_(2, 1.0, 0.0); // move right
|
||||
for (size_t x = 1; x < N; x++)
|
||||
for (size_t y = 1; y <= N; y++) {
|
||||
shared f(new Simulated2DOdometry(z1, sigma, key(x, y), key(x + 1, y)));
|
||||
nlfg.push_back(f);
|
||||
}
|
||||
|
||||
// Create vertical constraints, N*(N-1)+1..2*N*(N-1)
|
||||
Vector z2 = Vector_(2, 0.0, 1.0); // move up
|
||||
for (size_t x = 1; x <= N; x++)
|
||||
for (size_t y = 1; y < N; y++) {
|
||||
shared f(new Simulated2DOdometry(z2, sigma, key(x, y), key(x, y + 1)));
|
||||
nlfg.push_back(f);
|
||||
}
|
||||
|
||||
// Create linearization and ground xtrue config
|
||||
VectorConfig zeros, xtrue;
|
||||
for (size_t x = 1; x <= N; x++)
|
||||
for (size_t y = 1; y <= N; y++) {
|
||||
zeros.add(key(x, y), zero(2));
|
||||
xtrue.add(key(x, y), Vector_(2, (double) x, double(y)));
|
||||
}
|
||||
|
||||
// linearize around zero
|
||||
GaussianFactorGraph A = nlfg.linearize(zeros);
|
||||
|
||||
return make_pair(A, xtrue);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Ordering planarOrdering(size_t N) {
|
||||
Ordering ordering;
|
||||
for (size_t y = N; y >= 1; y--)
|
||||
for (size_t x = N; x >= 1; x--)
|
||||
ordering.push_back(key(x, y));
|
||||
return ordering;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<GaussianFactorGraph, GaussianFactorGraph> splitOffPlanarTree(size_t N,
|
||||
const GaussianFactorGraph& original) {
|
||||
GaussianFactorGraph T, C;
|
||||
|
||||
// Add the x11 constraint to the tree
|
||||
T.push_back(original[0]);
|
||||
|
||||
// Add all horizontal constraints to the tree
|
||||
size_t i = 1;
|
||||
for (size_t x = 1; x < N; x++)
|
||||
for (size_t y = 1; y <= N; y++, i++)
|
||||
T.push_back(original[i]);
|
||||
|
||||
// Add first vertical column of constraints to T, others to C
|
||||
for (size_t x = 1; x <= N; x++)
|
||||
for (size_t y = 1; y < N; y++, i++)
|
||||
if (x == 1)
|
||||
T.push_back(original[i]);
|
||||
else
|
||||
C.push_back(original[i]);
|
||||
|
||||
return make_pair(T, C);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -130,4 +130,38 @@ namespace gtsam {
|
|||
*/
|
||||
// ConstrainedNonlinearFactorGraph<NonlinearFactor<VectorConfig>,VectorConfig>
|
||||
// createConstrainedNonlinearFactorGraph();
|
||||
}
|
||||
|
||||
/* ******************************************************* */
|
||||
// Planar graph with easy subtree for SubgraphPreconditioner
|
||||
/* ******************************************************* */
|
||||
|
||||
/*
|
||||
* Create factor graph with N^2 nodes, for example for N=3
|
||||
* x13-x23-x33
|
||||
* | | |
|
||||
* x12-x22-x32
|
||||
* | | |
|
||||
* -x11-x21-x31
|
||||
* with x11 clamped at (1,1), and others related by 2D odometry.
|
||||
*/
|
||||
std::pair<GaussianFactorGraph, VectorConfig> planarGraph(size_t N);
|
||||
|
||||
/*
|
||||
* Create canonical ordering for planar graph that also works for tree
|
||||
* With x11 the root, e.g. for N=3
|
||||
* x33 x23 x13 x32 x22 x12 x31 x21 x11
|
||||
*/
|
||||
Ordering planarOrdering(size_t N);
|
||||
|
||||
/*
|
||||
* Split graph into tree and loop closing constraints, e.g., with N=3
|
||||
* x13-x23-x33
|
||||
* |
|
||||
* x12-x22-x32
|
||||
* |
|
||||
* -x11-x21-x31
|
||||
*/
|
||||
std::pair<GaussianFactorGraph, GaussianFactorGraph> splitOffPlanarTree(size_t N,
|
||||
const GaussianFactorGraph& original);
|
||||
|
||||
} // gtsam
|
||||
|
|
|
|||
Loading…
Reference in New Issue