Go to the documentation of this file.
24 *H1 = Matrix::Zero(1,6);
26 (*H1)(0, 3) =
pose.rotation().r1().z();
27 (*H1)(0, 4) =
pose.rotation().r2().z();
28 (*H1)(0, 5) =
pose.rotation().r3().z();
32 *H2 = Matrix::Zero(1,3);
46 std::cout <<
s <<
"RelativeElevationFactor, relative elevation = " <<
measured_ << std::endl;
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
const gtsam::Key pointKey
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Vector evaluateError(const Pose3 &pose, const Point3 &point, OptionalMatrixType H1, OptionalMatrixType H2) const override
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
noiseModel::Base::shared_ptr SharedNoiseModel
noiseModel::Diagonal::shared_ptr model
RelativeElevationFactor()
Matrix * OptionalMatrixType
std::uint64_t Key
Integer nonlinear key type.
Factor representing a known relative altitude in global frame.
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:34:56