testNonlinearClusterTree.cpp
Go to the documentation of this file.
1 
10 #include <gtsam/geometry/Pose2.h>
11 #include <gtsam/geometry/Point2.h>
12 
13 #include <gtsam/inference/Symbol.h>
14 
16 
17 using namespace gtsam;
18 
19 static const Symbol x1('x', 1), x2('x', 2), x3('x', 3);
20 static const Symbol l1('l', 1), l2('l', 2);
21 
22 /* ************************************************************************* */
25 
26  // Prior on pose x1 at the origin.
27  Pose2 prior(0.0, 0.0, 0.0);
28  auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
30 
31  // Two odometry factors
32  Pose2 odometry(2.0, 0.0, 0.0);
33  auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
36 
37  // Add Range-Bearing measurements to two different landmarks
38  auto measurementNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2));
39  Rot2 bearing11 = Rot2::fromDegrees(45), bearing21 = Rot2::fromDegrees(90),
40  bearing32 = Rot2::fromDegrees(90);
41  double range11 = std::sqrt(4.0 + 4.0), range21 = 2.0, range32 = 2.0;
42  graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x1, l1, bearing11, range11, measurementNoise);
43  graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x2, l1, bearing21, range21, measurementNoise);
44  graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x3, l2, bearing32, range32, measurementNoise);
45 
46  return graph;
47 }
48 
49 /* ************************************************************************* */
50 // Create initial estimate
53  initial.insert(l1, Point2(1.8, 2.1));
54  initial.insert(l2, Point2(4.1, 1.8));
55  initial.insert(x1, Pose2(0.5, 0.0, 0.2));
56  initial.insert(x2, Pose2(2.3, 0.1, -0.2));
57  initial.insert(x3, Pose2(4.1, 0.1, 0.1));
58  return initial;
59 }
60 
61 /* ************************************************************************* */
65 
66  // Build the clusters
67  // NOTE(frank): Order matters here as factors are removed!
68  VariableIndex variableIndex(graph);
70  auto marginalCluster = std::shared_ptr<Cluster>(new Cluster(variableIndex, {x1}, &graph));
71  auto landmarkCluster = std::shared_ptr<Cluster>(new Cluster(variableIndex, {l1, l2}, &graph));
72  auto rootCluster = std::shared_ptr<Cluster>(new Cluster(variableIndex, {x2, x3}, &graph));
73 
74  EXPECT_LONGS_EQUAL(3, marginalCluster->nrFactors());
75  EXPECT_LONGS_EQUAL(2, landmarkCluster->nrFactors());
76  EXPECT_LONGS_EQUAL(1, rootCluster->nrFactors());
77 
78  EXPECT_LONGS_EQUAL(1, marginalCluster->nrFrontals());
79  EXPECT_LONGS_EQUAL(2, landmarkCluster->nrFrontals());
80  EXPECT_LONGS_EQUAL(2, rootCluster->nrFrontals());
81 
82  // Test linearize
83  auto gfg = marginalCluster->linearize(initial);
84  EXPECT_LONGS_EQUAL(3, gfg->size());
85 
86  // Calculate expected result of only evaluating the marginalCluster
88  ordering.push_back(x1);
89  const auto [bn, fg] = gfg->eliminatePartialSequential(ordering);
90  auto expectedFactor = fg->at<HessianFactor>(0);
91  if (!expectedFactor)
92  throw std::runtime_error("Expected HessianFactor");
93 
94  // Linearize and eliminate the marginalCluster
95  auto actual = marginalCluster->linearizeAndEliminate(initial);
96  const GaussianBayesNet& bayesNet = actual.first;
97  const HessianFactor& factor = *actual.second;
98  EXPECT(assert_equal(*bn->at(0), *bayesNet.at(0), 1e-6));
99  EXPECT(assert_equal(*expectedFactor, factor, 1e-6));
100 }
101 
102 /* ************************************************************************* */
104  // Build the clusters
105  // NOTE(frank): Order matters here as factors are removed!
107  VariableIndex variableIndex(graph);
109  auto marginalCluster = std::shared_ptr<Cluster>(new Cluster(variableIndex, {x1}, &graph));
110  auto landmarkCluster = std::shared_ptr<Cluster>(new Cluster(variableIndex, {l1, l2}, &graph));
111  auto rootCluster = std::shared_ptr<Cluster>(new Cluster(variableIndex, {x2, x3}, &graph));
112 
113  // Build the tree
114  NonlinearClusterTree clusterTree;
115  clusterTree.addRoot(rootCluster);
116  rootCluster->addChild(landmarkCluster);
117  landmarkCluster->addChild(marginalCluster);
118 
119  return clusterTree;
120 }
121 
122 /* ************************************************************************* */
124  NonlinearClusterTree clusterTree = Construct();
125 
126  EXPECT_LONGS_EQUAL(3, clusterTree[0].problemSize());
127  EXPECT_LONGS_EQUAL(3, clusterTree[0][0].problemSize());
128  EXPECT_LONGS_EQUAL(3, clusterTree[0][0][0].problemSize());
129 }
130 
131 /* ************************************************************************* */
133  NonlinearClusterTree clusterTree = Construct();
134 
136  expected.insert(l1, Point2(2, 2));
137  expected.insert(l2, Point2(4, 2));
138  expected.insert(x1, Pose2(0, 0, 0));
139  expected.insert(x2, Pose2(2, 0, 0));
140  expected.insert(x3, Pose2(4, 0, 0));
141 
143  for (size_t i = 0; i < 4; i++)
144  values = clusterTree.updateCholesky(values);
145 
147 }
148 
149 /* ************************************************************************* */
150 int main() {
151  TestResult tr;
152  return TestRegistry::runAllTests(tr);
153 }
154 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::NonlinearClusterTree::NonlinearCluster
Definition: NonlinearClusterTree.h:18
Pose2.h
2D Pose
main
int main()
Definition: testNonlinearClusterTree.cpp:150
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:100
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
NonlinearClusterTree.h
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
TestHarness.h
initial
Values initial
Definition: OdometryOptimize.cpp:2
gtsam::NonlinearClusterTree
Definition: NonlinearClusterTree.h:14
gtsam::Rot2::fromDegrees
static Rot2 fromDegrees(double theta)
Named constructor from angle in degrees.
Definition: Rot2.h:66
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::noiseModel::Diagonal::Sigmas
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:291
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
Point2.h
2D Point
gtsam::FactorGraph::at
const sharedFactor at(size_t i) const
Definition: FactorGraph.h:306
odometry
Pose2 odometry(2.0, 0.0, 0.0)
gtsam::ClusterTree::addRoot
void addRoot(const std::shared_ptr< Cluster > &cluster)
Definition: ClusterTree.h:145
different_sigmas::bayesNet
const HybridBayesNet bayesNet
Definition: testHybridBayesNet.cpp:242
BetweenFactor.h
gtsam::BearingRangeFactor
Definition: sam/BearingRangeFactor.h:34
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
x2
static const Symbol x2('x', 2)
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
Symbol.h
x3
static const Symbol x3('x', 3)
planarSLAMValues
Values planarSLAMValues()
Definition: testNonlinearClusterTree.cpp:51
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
x1
static const Symbol x1('x', 1)
priorNoise
auto priorNoise
Definition: doc/Code/OdometryExample.cpp:6
gtsam::VariableIndex
Definition: VariableIndex.h:41
ordering
static enum @1096 ordering
TestResult
Definition: TestResult.h:26
planarSLAMGraph
NonlinearFactorGraph planarSLAMGraph()
Definition: testNonlinearClusterTree.cpp:23
gtsam::Rot2
Definition: Rot2.h:35
gtsam::NonlinearClusterTree::updateCholesky
Values updateCholesky(const Values &values)
Definition: NonlinearClusterTree.h:129
l2
static const Symbol l2('l', 2)
gtsam
traits
Definition: SFMdata.h:40
gtsam::TEST
TEST(SmartFactorBase, Pinhole)
Definition: testSmartFactorBase.cpp:38
gtsam::Values
Definition: Values.h:65
odometryNoise
auto odometryNoise
Definition: doc/Code/OdometryExample.cpp:11
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
initial
Definition: testScenarioRunner.cpp:148
different_sigmas::prior
const auto prior
Definition: testHybridBayesNet.cpp:238
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::Ordering
Definition: inference/Ordering.h:33
Construct
static NonlinearClusterTree Construct()
Definition: testNonlinearClusterTree.cpp:103
l1
static const Symbol l1('l', 1)
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::GaussianBayesNet
Definition: GaussianBayesNet.h:35
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
gtsam::Pose2
Definition: Pose2.h:39
gtsam::Symbol
Definition: inference/Symbol.h:37
BearingRangeFactor.h
a single factor contains both the bearing and the range to prevent handle to pair bearing and range f...
gtsam::BetweenFactor
Definition: BetweenFactor.h:40


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