30 using namespace gtsam;
32 int main(
int argc,
char** argv) {
44 auto model = noiseModel::Diagonal::Sigmas(
Vector3(0.2, 0.2, 0.1));
54 graph.
print(
"\nFactor Graph:\n");
64 initialEstimate.
insert(5,
Pose2(2.1, 2.1, -M_PI_2));
65 initialEstimate.
print(
"\nInitial Estimate:\n");
73 result.
print(
"Final Result:\n");
Matrix marginalCovariance(Key variable) const
virtual const Values & optimize()
T between(const T &t1, const T &t2)
noiseModel::Diagonal::shared_ptr model
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
NonlinearFactorGraph graph
int main(int argc, char **argv)
double relativeErrorTol
The maximum relative error decrease to stop iterating (default 1e-5)
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Factor graph that supports adding ExpressionFactors directly.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static ConjugateGradientParameters parameters
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
void print(const std::string &str="NonlinearFactorGraph: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
void insert(Key j, const Value &val)
void addExpressionFactor(const Expression< T > &h, const T &z, const SharedNoiseModel &R)
A class for computing marginals in a NonlinearFactorGraph.
Marginals marginals(graph, result)
size_t maxIterations
The maximum iterations to stop iterating (default 100)