31 using namespace gtsam;
39 Point2 x_initial(0.0, 0.0);
Rot2 R(Rot2::fromAngle(0.1))
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
JacobianFactor factor3(keyY, A32, keyZ, A33, b3, noiseModel::Isotropic::Sigma(2, sigma3))
T update(const NoiseModelFactor &measurementFactor)
Class to perform generic Kalman Filtering using nonlinear factor graphs.
T predict(const NoiseModelFactor &motionFactor)
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
noiseModel::Diagonal::shared_ptr SharedDiagonal
The quaternion class used to represent 3D orientations and rotations.
JacobianFactor factor2(keyX, A21, keyY, A22, b2, noiseModel::Isotropic::Sigma(2, sigma2))
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
JacobianFactor factor1(keyX, A11, b1, noiseModel::Isotropic::Sigma(2, sigma1))