33 using namespace gtsam;
46 Values linearizationPoints;
56 ordering->insert(x0, 0);
64 linearizationPoints.
insert(x0, x_initial);
65 linearFactorGraph->push_back(factor1.
linearize(linearizationPoints, *ordering));
89 ordering->insert(x1, 1);
95 linearizationPoints.
insert(x1, x_initial);
96 linearFactorGraph->push_back(factor2.
linearize(linearizationPoints, *ordering));
111 GaussianSequentialSolver solver0(*linearFactorGraph);
116 Point2 x1_predict = linearizationPoints.
at<
Point2>(
x1).retract(result[ordering->at(x1)]);
117 x1_predict.print(
"X1 Predict");
120 linearizationPoints.
update(x1, x1_predict);
141 assert(cg0->nrFrontals() == 1);
142 assert(cg0->nrParents() == 0);
143 linearFactorGraph->add(0, cg0->R(), cg0->d() - cg0->R()*result[ordering->at(x1)], noiseModel::Diagonal::Sigmas(cg0->get_sigmas(),
true));
147 ordering->insert(x1, 0);
173 linearFactorGraph->push_back(factor4.
linearize(linearizationPoints, *ordering));
183 GaussianSequentialSolver solver1(*linearFactorGraph);
184 linearBayesNet = solver1.eliminate();
188 Point2 x1_update = linearizationPoints.
at<
Point2>(
x1).retract(result[ordering->at(x1)]);
189 x1_update.print(
"X1 Update");
192 linearizationPoints.
update(x1, x1_update);
207 assert(cg1->nrFrontals() == 1);
208 assert(cg1->nrParents() == 0);
210 linearFactorGraph->add(0, tmpPrior1.getA(tmpPrior1.begin()), tmpPrior1.getb() - tmpPrior1.getA(tmpPrior1.begin()) * result[ordering->at(x1)], tmpPrior1.get_model());
217 ordering->insert(x1, 0);
218 ordering->insert(x2, 1);
222 Q = noiseModel::Diagonal::Sigmas((
Vec(2) <, 0.1, 0.1));
226 linearizationPoints.
insert(x2, x1_update);
227 linearFactorGraph->push_back(factor6.
linearize(linearizationPoints, *ordering));
230 GaussianSequentialSolver solver2(*linearFactorGraph);
231 linearBayesNet = solver2.eliminate();
235 Point2 x2_predict = linearizationPoints.
at<
Point2>(
x2).retract(result[ordering->at(x2)]);
236 x2_predict.print(
"X2 Predict");
239 linearizationPoints.
update(x2, x2_predict);
249 assert(cg2->nrFrontals() == 1);
250 assert(cg2->nrParents() == 0);
252 linearFactorGraph->add(0, tmpPrior2.getA(tmpPrior2.begin()), tmpPrior2.getb() - tmpPrior2.getA(tmpPrior2.begin()) * result[ordering->at(x2)], tmpPrior2.get_model());
256 ordering->insert(x2, 0);
264 linearFactorGraph->push_back(factor8.
linearize(linearizationPoints, *ordering));
274 GaussianSequentialSolver solver3(*linearFactorGraph);
275 linearBayesNet = solver3.eliminate();
279 Point2 x2_update = linearizationPoints.
at<
Point2>(
x2).retract(result[ordering->at(x2)]);
280 x2_update.print(
"X2 Update");
283 linearizationPoints.
update(x2, x2_update);
296 assert(cg3->nrFrontals() == 1);
297 assert(cg3->nrParents() == 0);
299 linearFactorGraph->add(0, tmpPrior3.getA(tmpPrior3.begin()), tmpPrior3.getb() - tmpPrior3.getA(tmpPrior3.begin()) * result[ordering->at(x2)], tmpPrior3.get_model());
306 ordering->insert(x2, 0);
307 ordering->insert(x3, 1);
311 Q = noiseModel::Diagonal::Sigmas((
Vec(2) << 0.1, 0.1));
315 linearizationPoints.
insert(x3, x2_update);
316 linearFactorGraph->push_back(factor10.
linearize(linearizationPoints, *ordering));
319 GaussianSequentialSolver solver4(*linearFactorGraph);
320 linearBayesNet = solver4.eliminate();
324 Point2 x3_predict = linearizationPoints.
at<
Point2>(
x3).retract(result[ordering->at(x3)]);
325 x3_predict.print(
"X3 Predict");
328 linearizationPoints.
update(x3, x3_predict);
338 assert(cg4->nrFrontals() == 1);
339 assert(cg4->nrParents() == 0);
341 linearFactorGraph->add(0, tmpPrior4.getA(tmpPrior4.begin()), tmpPrior4.getb() - tmpPrior4.getA(tmpPrior4.begin()) * result[ordering->at(x3)], tmpPrior4.get_model());
345 ordering->insert(x3, 0);
353 linearFactorGraph->push_back(factor12.
linearize(linearizationPoints, *ordering));
363 GaussianSequentialSolver solver5(*linearFactorGraph);
364 linearBayesNet = solver5.eliminate();
368 Point2 x3_update = linearizationPoints.
at<
Point2>(
x3).retract(result[ordering->at(x3)]);
369 x3_update.print(
"X3 Update");
372 linearizationPoints.
update(x3, x3_update);
boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override
void insert(Key j, const Value &val)
static enum @843 ordering
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
boost::shared_ptr< This > shared_ptr
const ValueType at(Key j) const
Linear Factor Graph where all factors are Gaussians.
Q R1(Eigen::AngleAxisd(1, Q_z_axis))
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
noiseModel::Diagonal::shared_ptr SharedDiagonal
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
The quaternion class used to represent 3D orientations and rotations.
int optimize(const SfmData &db, const NonlinearFactorGraph &graph, const Values &initial, bool separateCalibration=false)
JacobianFactor factor2(keyX, A21, keyY, A22, b2, noiseModel::Isotropic::Sigma(2, sigma2))
Chordal Bayes Net, the result of eliminating a factor graph.
void update(Key j, const Value &val)
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Q R2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)))
Matrix< SCALAR, Eigen::Dynamic, 1 > Vec
JacobianFactor factor1(keyX, A11, b1, noiseModel::Isotropic::Sigma(2, sigma1))