26 using namespace gtsam;
90 Matrix H1Expected, H2Expected;
91 H1Expected = numericalDerivative11<Vector, Point3>(
93 H2Expected = numericalDerivative11<Vector, Point3>(
static int runAllTests(TestResult &result)
TEST(TranslationFactor, Constructor)
EIGEN_DEVICE_FUNC const SqrtReturnType sqrt() const
Pose2 T2(M_PI/2.0, Point2(0.0, 2.0))
Some functions to compute numerical derivatives.
static const Key kKey2(1)
Represents a 3D point on a unit sphere.
Vector evaluateError(const Point3 &Ta, const Point3 &Tb, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Caclulate error: (norm(Tb - Ta) - measurement) where Tb and Ta are Point3 translations and measuremen...
#define EXPECT(condition)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Binary factor for a relative translation direction measurement.
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
static const Key kKey1(2)
Pose2 T1(M_PI/4.0, Point2(sqrt(0.5), sqrt(0.5)))
Vector factorError(const Point3 &T1, const Point3 &T2, const TranslationFactor &factor)
static const Unit3 kMeasured(1, 0, 0)
std::uint64_t Key
Integer nonlinear key type.
noiseModel::Base::shared_ptr SharedNoiseModel
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.05))