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

gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:40:41