Go to the documentation of this file.
35 typedef gtsam::NoiseModelFactor2<PoseRTV,PoseRTV>
Base;
38 using Base::evaluateError;
51 double dt,
double mu = 1000)
80 return std::static_pointer_cast<gtsam::NonlinearFactor>(
88 if (H1) *H1 = gtsam::numericalDerivative21<gtsam::Vector,PoseRTV,PoseRTV>(
91 if (H2) *H2 = gtsam::numericalDerivative22<gtsam::Vector,PoseRTV,PoseRTV>(
98 std::string
a =
"VelocityConstraint: " +
s;
104 default: std::cout <<
"Integration: Unknown\n" << std::endl;
break;
106 std::cout <<
"dt: " <<
dt_ << std::endl;
120 default: assert(
false);
break;
std::shared_ptr< This > shared_ptr
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
dynamics::IntegrationMode integration_mode_
time difference between frames in seconds
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Vector3 Velocity3
Velocity is currently typedef'd to Vector3.
gtsam::NoiseModelFactor2< PoseRTV, PoseRTV > Base
const KeyFormatter & formatter
VelocityConstraint(Key key1, Key key2, double dt, double mu=1000)
void print(const std::string &s="", const gtsam::KeyFormatter &formatter=gtsam::DefaultKeyFormatter) const override
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
VelocityConstraint(Key key1, Key key2, const dynamics::IntegrationMode &mode, double dt, double mu=1000)
Some functions to compute numerical derivatives.
VelocityConstraint(Key key1, Key key2, double dt, const gtsam::SharedNoiseModel &model)
~VelocityConstraint() override
static gtsam::Vector evaluateError_(const PoseRTV &x1, const PoseRTV &x2, double dt, const dynamics::IntegrationMode &mode)
const SharedNoiseModel & noiseModel() const
access to the noise model
static const DiscreteKey mode(modeKey, 2)
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Pose3 with translational velocity.
VelocityConstraint(Key key1, Key key2, const dynamics::IntegrationMode &mode, double dt, const gtsam::SharedNoiseModel &model)
noiseModel::Base::shared_ptr SharedNoiseModel
noiseModel::Diagonal::shared_ptr model
Non-linear factor base classes.
gtsam::Vector evaluateError(const PoseRTV &x1, const PoseRTV &x2, OptionalMatrixType H1, OptionalMatrixType H2) const override
Matrix * OptionalMatrixType
std::uint64_t Key
Integer nonlinear key type.
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
gtsam::NonlinearFactor::shared_ptr clone() const override
gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:09:42