17 using namespace gtsam;
41 double range11 =
std::sqrt(4.0 + 4.0), range21 = 2.0, range32 = 2.0;
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));
83 auto gfg = marginalCluster->linearize(
initial);
89 const auto [bn, fg] = gfg->eliminatePartialSequential(
ordering);
92 throw std::runtime_error(
"Expected HessianFactor");
95 auto actual = marginalCluster->linearizeAndEliminate(
initial);
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));
115 clusterTree.
addRoot(rootCluster);
116 rootCluster->addChild(landmarkCluster);
117 landmarkCluster->addChild(marginalCluster);
143 for (
size_t i = 0;
i < 4;
i++)