testNestedDissection.cpp
Go to the documentation of this file.
1 /*
2  * testNestedDissection.cpp
3  *
4  * Created on: Nov 29, 2010
5  * Author: nikai
6  * Description: unit tests for NestedDissection
7  */
8 
9 #include <boost/assign/std/list.hpp> // for operator +=
10 #include <boost/assign/std/set.hpp> // for operator +=
11 #include <boost/assign/std/vector.hpp> // for operator +=
12 using namespace boost::assign;
13 #include <boost/make_shared.hpp>
14 #include <CppUnitLite/TestHarness.h>
15 
16 #include "SubmapPlanarSLAM.h"
17 #include "SubmapVisualSLAM.h"
18 #include "SubmapExamples.h"
19 #include "SubmapExamples3D.h"
20 #include "GenericGraph.h"
21 #include "NonlinearTSAM.h"
22 #include "partition/NestedDissection-inl.h"
23 
24 using namespace std;
25 using namespace gtsam;
26 using namespace gtsam::partition;
27 
28 /* ************************************************************************* */
29 // x1 - x2
30 // \ /
31 // l1
32 TEST ( NestedDissection, oneIsland )
33 {
34  using namespace submapPlanarSLAM;
35  typedef TSAM2D::SubNLG SubNLG;
36  Graph fg;
37  fg.addOdometry(1, 2, Pose2(), odoNoise);
38  fg.addBearingRange(1, 1, Rot2(), 0., bearingRangeNoise);
39  fg.addBearingRange(2, 1, Rot2(), 0., bearingRangeNoise);
40  fg.addPoseConstraint(1, Pose2());
41 
42  Ordering ordering; ordering += x1, x2, l1;
43 
44  int numNodeStopPartition = 1e3;
45  int minNodesPerMap = 1e3;
46  NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
47  LONGS_EQUAL(4, nd.root()->size());
48  LONGS_EQUAL(3, nd.root()->frontal().size());
49  LONGS_EQUAL(0, nd.root()->children().size());
50 }
51 
52 /* ************************************************************************* */
53 // x1\ /x4
54 // | x3 |
55 // x2/ \x5
56 TEST ( NestedDissection, TwoIslands )
57 {
58  using namespace submapPlanarSLAM;
59  typedef TSAM2D::SubNLG SubNLG;
60  Graph fg;
61  fg.addOdometry(1, 2, Pose2(), odoNoise);
62  fg.addOdometry(1, 3, Pose2(), odoNoise);
63  fg.addOdometry(2, 3, Pose2(), odoNoise);
64  fg.addOdometry(3, 4, Pose2(), odoNoise);
65  fg.addOdometry(4, 5, Pose2(), odoNoise);
66  fg.addOdometry(3, 5, Pose2(), odoNoise);
67  fg.addPoseConstraint(1, Pose2());
68  fg.addPoseConstraint(4, Pose2());
69  Ordering ordering; ordering += x1, x2, x3, x4, x5;
70 
71  int numNodeStopPartition = 2;
72  int minNodesPerMap = 1;
73  NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
74  // root submap
75  LONGS_EQUAL(0, nd.root()->size());
76  LONGS_EQUAL(1, nd.root()->frontal().size());
77  LONGS_EQUAL(0, nd.root()->separator().size());
78  LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps
79 
80  // the 1st submap
81  LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size());
82  LONGS_EQUAL(4, nd.root()->children()[0]->size());
83 
84  // the 2nd submap
85  LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size());
86  LONGS_EQUAL(4, nd.root()->children()[1]->size());
87 }
88 
89 /* ************************************************************************* */
90 // x1\ /x4
91 // x3
92 // x2/ \x5
93 TEST ( NestedDissection, FourIslands )
94 {
95  using namespace submapPlanarSLAM;
96  typedef TSAM2D::SubNLG SubNLG;
97  Graph fg;
98  fg.addOdometry(1, 3, Pose2(), odoNoise);
99  fg.addOdometry(2, 3, Pose2(), odoNoise);
100  fg.addOdometry(3, 4, Pose2(), odoNoise);
101  fg.addOdometry(3, 5, Pose2(), odoNoise);
102  fg.addPoseConstraint(1, Pose2());
103  fg.addPoseConstraint(4, Pose2());
104  Ordering ordering; ordering += x1, x2, x3, x4, x5;
105 
106  int numNodeStopPartition = 2;
107  int minNodesPerMap = 1;
108  NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
109  LONGS_EQUAL(0, nd.root()->size());
110  LONGS_EQUAL(1, nd.root()->frontal().size());
111  LONGS_EQUAL(0, nd.root()->separator().size());
112  LONGS_EQUAL(4, nd.root()->children().size()); // 4 leaf submaps
113 
114  // the 1st submap
115  LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size());
116  LONGS_EQUAL(2, nd.root()->children()[0]->size());
117 
118  // the 2nd submap
119  LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size());
120  LONGS_EQUAL(2, nd.root()->children()[1]->size());
121 
122  // the 3rd submap
123  LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size());
124  LONGS_EQUAL(1, nd.root()->children()[2]->size());
125 
126  // the 4th submap
127  LONGS_EQUAL(1, nd.root()->children()[3]->frontal().size());
128  LONGS_EQUAL(1, nd.root()->children()[3]->size());
129 }
130 
131 /* ************************************************************************* */
132 // x1\ /x3
133 // | x2 |
134 // l6/ \x4
135 // |
136 // x5
137 TEST ( NestedDissection, weekLinks )
138 {
139  using namespace submapPlanarSLAM;
140  typedef TSAM2D::SubNLG SubNLG;
141  Graph fg;
142  fg.addOdometry(1, 2, Pose2(), odoNoise);
143  fg.addOdometry(2, 3, Pose2(), odoNoise);
144  fg.addOdometry(2, 4, Pose2(), odoNoise);
145  fg.addOdometry(3, 4, Pose2(), odoNoise);
146  fg.addBearingRange(1, 6, Rot2(), 0., bearingRangeNoise);
147  fg.addBearingRange(2, 6, Rot2(), 0., bearingRangeNoise);
148  fg.addBearingRange(5, 6, Rot2(), 0., bearingRangeNoise);
149  fg.addPoseConstraint(1, Pose2());
150  fg.addPoseConstraint(4, Pose2());
151  fg.addPoseConstraint(5, Pose2());
152  Ordering ordering; ordering += x1, x2, x3, x4, x5, l6;
153 
154  int numNodeStopPartition = 2;
155  int minNodesPerMap = 1;
156  NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
157  LONGS_EQUAL(0, nd.root()->size()); // one weeklink
158  LONGS_EQUAL(1, nd.root()->frontal().size());
159  LONGS_EQUAL(0, nd.root()->separator().size());
160  LONGS_EQUAL(3, nd.root()->children().size()); // 4 leaf submaps
161  LONGS_EQUAL(1, nd.root()->weeklinks().size());
162 
163  // the 1st submap
164  LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size()); // x3 and x4
165  LONGS_EQUAL(4, nd.root()->children()[0]->size());
166 
167  // the 2nd submap
168  LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size()); // x1 and l6
169  LONGS_EQUAL(4, nd.root()->children()[1]->size());
170  //
171  // the 3rd submap
172  LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size()); // x5
173  LONGS_EQUAL(1, nd.root()->children()[2]->size());
174 }
175 
176 /* ************************************************************************* */
184 TEST ( NestedDissection, manual_cuts )
185 {
186  using namespace submapPlanarSLAM;
187  typedef partition::Cuts Cuts;
188  typedef TSAM2D::SubNLG SubNLG;
189  typedef partition::PartitionTable PartitionTable;
190  Graph fg;
191  fg.addOdometry(x0, x1, Pose2(1.0, 0, 0), odoNoise);
192  fg.addOdometry(x1, x2, Pose2(1.0, 0, 0), odoNoise);
193 
194  fg.addBearingRange(x0, l1, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise);
195  fg.addBearingRange(x0, l4, Rot2::fromAngle(-M_PI_2), 1, bearingRangeNoise);
196  fg.addBearingRange(x0, l2, Rot2::fromAngle( M_PI_4), sqrt(2), bearingRangeNoise);
197  fg.addBearingRange(x0, l5, Rot2::fromAngle(-M_PI_4), sqrt(2), bearingRangeNoise);
198 
199  fg.addBearingRange(x1, l1, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise);
200  fg.addBearingRange(x1, l2, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise);
201  fg.addBearingRange(x1, l3, Rot2::fromAngle( M_PI_4), sqrt(2), bearingRangeNoise);
202  fg.addBearingRange(x1, l4, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise);
203  fg.addBearingRange(x1, l5, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise);
204  fg.addBearingRange(x1, l6, Rot2::fromAngle(-M_PI_4), sqrt(2), bearingRangeNoise);
205 
206  fg.addBearingRange(x2, l2, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise);
207  fg.addBearingRange(x2, l5, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise);
208  fg.addBearingRange(x2, l3, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise);
209  fg.addBearingRange(x2, l6, Rot2::fromAngle(-M_PI_2), 1, bearingRangeNoise);
210 
211  fg.addPrior(x0, Pose2(0.1, 0, 0), priorNoise);
212 
213  // generate ordering
214  Ordering ordering; ordering += x0, x1, x2, l1, l2, l3, l4, l5, l6;
215 
216  // define cuts
217  boost::shared_ptr<Cuts> cuts(new Cuts());
218  cuts->partitionTable = PartitionTable(9, -1); PartitionTable* p = &cuts->partitionTable;
219  //x0 x1 x2 l1 l2 l3 l4 l5 l6
220  (*p)[0]=1; (*p)[1]=0; (*p)[2]=2; (*p)[3]=1; (*p)[4]=0; (*p)[5]=2; (*p)[6]=1; (*p)[7]=0; (*p)[8]=2;
221 
222  cuts->children.push_back(boost::shared_ptr<Cuts>(new Cuts()));
223  cuts->children[0]->partitionTable = PartitionTable(9, -1); p = &cuts->children[0]->partitionTable;
224  //x0 x1 x2 l1 l2 l3 l4 l5 l6
225  (*p)[0]=0; (*p)[1]=-1; (*p)[2]=-1; (*p)[3]=1; (*p)[4]=-1; (*p)[5]=-1; (*p)[6]=2; (*p)[7]=-1; (*p)[8]=-1;
226 
227  cuts->children.push_back(boost::shared_ptr<Cuts>(new Cuts()));
228  cuts->children[1]->partitionTable = PartitionTable(9, -1); p = &cuts->children[1]->partitionTable;
229  //x0 x1 x2 l1 l2 l3 l4 l5 l6
230  (*p)[0]=-1; (*p)[1]=-1; (*p)[2]=0; (*p)[3]=-1; (*p)[4]=-1; (*p)[5]=1; (*p)[6]=-1; (*p)[7]=-1; (*p)[8]=2;
231 
232 
233  // nested dissection
234  NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, cuts);
235  LONGS_EQUAL(2, nd.root()->size());
236  LONGS_EQUAL(3, nd.root()->frontal().size());
237  LONGS_EQUAL(0, nd.root()->separator().size());
238  LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps
239  LONGS_EQUAL(0, nd.root()->weeklinks().size());
240 
241  // the 1st submap
242  LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size()); // x0
243  LONGS_EQUAL(4, nd.root()->children()[0]->size());
244  LONGS_EQUAL(2, nd.root()->children()[0]->children().size());
245 
246  // the 1-1st submap
247  LONGS_EQUAL(1, nd.root()->children()[0]->children()[0]->frontal().size()); // l1
248  LONGS_EQUAL(2, nd.root()->children()[0]->children()[0]->size());
249 
250  // the 1-2nd submap
251  LONGS_EQUAL(1, nd.root()->children()[0]->children()[1]->frontal().size()); // l4
252  LONGS_EQUAL(2, nd.root()->children()[0]->children()[1]->size());
253 
254  // the 2nd submap
255  LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size()); // x2
256  LONGS_EQUAL(3, nd.root()->children()[1]->size());
257  LONGS_EQUAL(2, nd.root()->children()[1]->children().size());
258 
259  // the 2-1st submap
260  LONGS_EQUAL(1, nd.root()->children()[1]->children()[0]->frontal().size()); // l3
261  LONGS_EQUAL(2, nd.root()->children()[1]->children()[0]->size());
262 
263  // the 2-2nd submap
264  LONGS_EQUAL(1, nd.root()->children()[1]->children()[1]->frontal().size()); // l6
265  LONGS_EQUAL(2, nd.root()->children()[1]->children()[1]->size());
266 
267 }
268 
269 /* ************************************************************************* */
270 // l1-l8 l9-16 l17-124
271 // / | / \ | \
272 // x0 x1 x2 x3
273 TEST( NestedDissection, Graph3D) {
274  using namespace gtsam::submapVisualSLAM;
275  typedef TSAM3D::SubNLG SubNLG;
276  typedef partition::PartitionTable PartitionTable;
277  vector<GeneralCamera> cameras;
278  cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-2., 0., 0.))));
279  cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-1., 0., 0.))));
280  cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 1., 0., 0.))));
281  cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 2., 0., 0.))));
282 
283  vector<Point3> points;
284  for (int cube_index = 0; cube_index <= 3; cube_index++) {
285  Point3 center((cube_index-1) * 3, 0.5, 10.);
286  points.push_back(center + Point3(-0.5, -0.5, -0.5));
287  points.push_back(center + Point3(-0.5, 0.5, -0.5));
288  points.push_back(center + Point3( 0.5, 0.5, -0.5));
289  points.push_back(center + Point3( 0.5, -0.5, -0.5));
290  points.push_back(center + Point3(-0.5, -0.5, 0.5));
291  points.push_back(center + Point3(-0.5, 0.5, 0.5));
292  points.push_back(center + Point3( 0.5, 0.5, 0.5));
293  points.push_back(center + Point3( 0.5, 0.5, 0.5));
294  }
295 
296  Graph graph;
297  SharedDiagonal measurementNoise(gtsam::Vector_(2, 1., 1.));
298  SharedDiagonal measurementZeroNoise(gtsam::Vector_(2, 0., 0.));
299  for (int j=1; j<=8; j++)
300  graph.addMeasurement(0, j, cameras[0].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
301  for (int j=1; j<=16; j++)
302  graph.addMeasurement(1, j, cameras[1].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
303  for (int j=9; j<=24; j++)
304  graph.addMeasurement(2, j, cameras[2].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
305  for (int j=17; j<=24; j++)
306  graph.addMeasurement(3, j, cameras[3].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
307 
308  // make an easy ordering
309  Ordering ordering; ordering += x0, x1, x2, x3;
310  for (int j=1; j<=24; j++)
311  ordering += Symbol('l', j);
312 
313  // nested dissection
314  const int numNodeStopPartition = 10;
315  const int minNodesPerMap = 5;
316  NestedDissection<Graph, SubNLG, GenericGraph3D> nd(graph, ordering, numNodeStopPartition, minNodesPerMap);
317 
318  LONGS_EQUAL(0, nd.root()->size());
319  LONGS_EQUAL(8, nd.root()->frontal().size()); // l9-l16
320  LONGS_EQUAL(0, nd.root()->separator().size());
321  LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps
322  LONGS_EQUAL(0, nd.root()->weeklinks().size());
323 
324  // the 1st submap
325  LONGS_EQUAL(10, nd.root()->children()[0]->frontal().size()); // x0, x1, l1-l8
326  LONGS_EQUAL(24, nd.root()->children()[0]->size()); // 8 + 16
327  LONGS_EQUAL(0, nd.root()->children()[0]->children().size());
328 
329  // the 2nd submap
330  LONGS_EQUAL(10, nd.root()->children()[1]->frontal().size()); // x2, x3, l1-l8
331  LONGS_EQUAL(24, nd.root()->children()[1]->size()); // 16 + 8
332  LONGS_EQUAL(0, nd.root()->children()[1]->children().size());
333 }
334 
335 
336 /* ************************************************************************* */
337 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
338 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
ordering
static enum @843 ordering
l2
gtsam::Key l2
Definition: testLinearContainerFactor.cpp:25
sqrt
EIGEN_DEVICE_FUNC const SqrtReturnType sqrt() const
Definition: ArrayCwiseUnaryOps.h:152
std
Definition: Half.h:150
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
x5
static string x5("x5")
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
TEST
TEST(NestedDissection, oneIsland)
Definition: testNestedDissection.cpp:32
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:229
main
int main()
Definition: testNestedDissection.cpp:337
odoNoise
SharedDiagonal odoNoise
Definition: testGaussianISAM2.cpp:43
l3
static const Symbol l3('l', 3)
x3
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
l4
Point3 l4(1, 4,-4)
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:135
l1
gtsam::Key l1
Definition: testLinearContainerFactor.cpp:25
x0
static Symbol x0('x', 0)
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:736
gtsam
traits
Definition: chartTesting.h:28
gtsam::partition::PartitionTable
std::vector< int > PartitionTable
Definition: PartitionWorkSpace.h:16
Graph::addMeasurement
void addMeasurement(int i, int j, const Point2 &z, const SharedNoiseModel &model)
Definition: testGeneralSFMFactor.cpp:53
x1
Pose3 x1
Definition: testPose3.cpp:588
p
float * p
Definition: Tutorial_Map_using.cpp:9
priorNoise
noiseModel::Diagonal::shared_ptr priorNoise
Definition: doc/Code/OdometryExample.cpp:6
gtsam::Point3
Vector3 Point3
Definition: Point3.h:35
x4
static string x4("x4")
GeneralCamera
PinholeCamera< Cal3_S2 > GeneralCamera
Definition: testGeneralSFMFactor.cpp:46
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:48:08