Go to the documentation of this file.
30 using namespace gtsam;
37 if (Hv) *Hv =
s * I_3x3;
50 int main(
int argc,
char** argv) {
52 const double L1 = 3.5, L2 = 3.5, L3 = 2.5;
53 const Pose2 sXt0(0, L1 + L2 + L3,
M_PI / 2);
54 const Vector3 xi1(0, 0, 1), xi2(L1, 0, 1),
69 Pose2 desiredEndEffectorPose(3, 2, 0);
70 auto model = noiseModel::Diagonal::Sigmas(
Vector3(0.2, 0.2, 0.1));
78 initial.print(
"\nInitial Estimate:\n");
83 params.setlambdaInitial(1e6);
virtual const Values & optimize()
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Expression< Pose2 > Pose2_
static const SmartProjectionParams params
Pose2_ Expmap(const Vector3_ &xi)
Common expressions, both linear and non-linear.
Factor graph that supports adding ExpressionFactors directly.
int main(int argc, char **argv)
noiseModel::Diagonal::shared_ptr model
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
A class for computing marginals in a NonlinearFactorGraph.
Common expressions for solving geometry/slam/sfm problems.
T compose(const T &t1, const T &t2)
Array< int, Dynamic, 1 > v
Expression< Vector3 > Vector3_
NonlinearFactorGraph graph
Vector3 scalarMultiply(const double &s, const Vector3 &v, OptionalJacobian< 3, 1 > Hs, OptionalJacobian< 3, 3 > Hv)
Vector3_ operator*(const Double_ &s, const Vector3_ &v)
void addExpressionFactor(const SharedNoiseModel &R, const T &z, const Expression< T > &h)
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:51