27 using namespace gtsam;
49 Matrix H1Expected, H2Expected;
51 H1Expected = numericalDerivative11<Vector2, Pose2>(
52 [&factor, &
point](
const Pose2&
pose) {
return factor.evaluateError(pose, point); },
pose);
53 H2Expected = numericalDerivative11<Vector2, Point2>(
70 Pose2 base1, base2(1, 0, 0);
76 Matrix H1Actual, H2Actual, H3Actual, H4Actual;
78 H2Actual, H3Actual, H4Actual);
82 Matrix H1Expected, H2Expected, H3Expected, H4Expected;
83 H1Expected = numericalDerivative11<Vector2, Pose2>(
85 return factor.evaluateError(pose_arg, pose, base2, point);
88 H2Expected = numericalDerivative11<Vector2, Pose2>(
89 [&factor, &
point, &base1, &base2](
const Pose2& pose_arg) {
90 return factor.evaluateError(base1, pose_arg, base2, point);
93 H3Expected = numericalDerivative11<Vector2, Pose2>(
95 return factor.evaluateError(base1, pose, pose_arg, point);
98 H4Expected = numericalDerivative11<Vector2, Point2>(
99 [&factor, &
pose, &base1, &base2](
const Point2& point_arg) {
100 return factor.evaluateError(base1, pose, base2, point_arg);
120 Pose2 base1, base2(1, 0, 0);
125 Matrix H1Actual, H2Actual, H3Actual, H4Actual;
127 H2Actual, H3Actual, H4Actual);
131 Matrix H1Expected, H2Expected, H3Expected, H4Expected;
133 H1Expected = numericalDerivative11<Vector3, Pose2>(
135 return factor.evaluateError(pose_arg, pose1, base2, pose2);
138 H2Expected = numericalDerivative11<Vector3, Pose2>(
139 [&factor, &
pose2, &base1, &base2](
const Pose2& pose_arg) {
140 return factor.evaluateError(base1, pose_arg, base2, pose2);
143 H3Expected = numericalDerivative11<Vector3, Pose2>(
145 return factor.evaluateError(base1, pose1, pose_arg, pose2);
148 H4Expected = numericalDerivative11<Vector3, Pose2>(
149 [&factor, &
pose1, &base1, &base2](
const Pose2& pose_arg) {
150 return factor.evaluateError(base1, pose1, base2, pose_arg);
Vector evaluateError(const Pose2 &pose, const Point2 &point, OptionalMatrixType H1, OptionalMatrixType H2) const override
Evaluate measurement error h(x)-z.
static const Eigen::internal::all_t all
static int runAllTests(TestResult &result)
noiseModel::Diagonal::shared_ptr model
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Vector evaluateError(const Pose2 &base1, const Pose2 &pose1, const Pose2 &base2, const Pose2 &pose2, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, OptionalMatrixType H4) const override
Evaluate measurement error h(x)-z.
Some functions to compute numerical derivatives.
TSAM 1 Factors, simpler than the hierarchical TSAM 2.
static Point2 measurement(323.0, 240.0)
#define EXPECT(condition)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
Vector evaluateError(const Pose2 &base1, const Pose2 &pose, const Pose2 &base2, const Point2 &point, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, OptionalMatrixType H4) const override
Evaluate measurement error h(x)-z.
std::uint64_t Key
Integer nonlinear key type.
noiseModel::Base::shared_ptr SharedNoiseModel