30 using namespace gtsam;
static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(3, 0.5))
static int runAllTests(TestResult &result)
void insert(Key j, const Value &val)
BearingRangeFactor2D factor2D(poseKey, pointKey, 1, 2, model2D)
BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic)
bool equalsObj(const T &input=T())
Evaluate derivatives of a nonlinear factor numerically.
static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(2, 0.5))
bool equalsXML(const T &input=T())
bool equalsBinary(const T &input=T())
#define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, numerical_derivative_step, tolerance)
Check the Jacobians produced by an expression against finite differences.
void deserializeXML(const std::string &serialized, T &output, const std::string &name="data")
deserializes from a string in XML
BearingRangeFactor< Pose3, Point3 > BearingRangeFactor3D
Test harness methods for expressions.
#define EXPECT(condition)
TEST(BearingRangeFactor, Serialization2D)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
std::string serializeXML(const T &input, const std::string &name="data")
serializes to a string in XML
BearingRangeFactor3D factor3D(poseKey, pointKey, Pose3().bearing(Point3(1, 0, 0)), 1, model3D)
#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance)
Check the Jacobians produced by a factor against finite differences.
BearingRangeFactor< Pose2, Point2 > BearingRangeFactor2D
std::uint64_t Key
Integer nonlinear key type.
noiseModel::Base::shared_ptr SharedNoiseModel
Expression< T > expression(const typename Base::ArrayNKeys &keys) const override