29 using namespace std::placeholders;
31 using namespace gtsam;
106 Point3(0.25, -0.10, 1.0));
130 Vector actualError(
factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}));
133 Vector expectedError = (
Vector(1) << 0.295630141).finished();
153 Vector actualError(
factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}));
156 Vector expectedError = (
Vector(1) << 0.295630141).finished();
172 Vector actualError(
factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}));
175 Vector expectedError = (
Vector(1) << 0.295630141).finished();
185 Point3(0.25, -0.10, 1.0));
190 Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75);
196 Vector actualError(
factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}));
199 Vector expectedError = (
Vector(1) << 0.295630141).finished();
215 Matrix H1Actual, H2Actual;
219 Matrix H1Expected, H2Expected;
220 H1Expected = numericalDerivative11<Vector, Pose2>(
221 std::bind(&factorError2D, std::placeholders::_1,
point,
factor),
pose);
222 H2Expected = numericalDerivative11<Vector, Point2>(
223 std::bind(&factorError2D,
pose, std::placeholders::_1,
factor),
point);
244 std::vector<Matrix> actualHs(2);
246 const Matrix& H1Actual = actualHs.at(0);
247 const Matrix& H2Actual = actualHs.at(1);
250 Matrix H1Expected, H2Expected;
251 H1Expected = numericalDerivative11<Vector, Pose2>(
252 std::bind(&factorErrorWithTransform2D, std::placeholders::_1,
point,
factor),
pose);
253 H2Expected = numericalDerivative11<Vector, Point2>(
254 std::bind(&factorErrorWithTransform2D,
pose, std::placeholders::_1,
factor),
point);
271 std::vector<Matrix> actualHs(2);
273 const Matrix& H1Actual = actualHs.at(0);
274 const Matrix& H2Actual = actualHs.at(1);
277 Matrix H1Expected, H2Expected;
278 H1Expected = numericalDerivative11<Vector, Pose3>(
279 std::bind(&factorError3D, std::placeholders::_1,
point,
factor),
pose);
280 H2Expected = numericalDerivative11<Vector, Point3>(
281 std::bind(&factorError3D,
pose, std::placeholders::_1,
factor),
point);
292 Point3(0.25, -0.10, 1.0));
297 Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75);
303 std::vector<Matrix> actualHs(2);
305 const Matrix& H1Actual = actualHs.at(0);
306 const Matrix& H2Actual = actualHs.at(1);
309 Matrix H1Expected, H2Expected;
310 H1Expected = numericalDerivative11<Vector, Pose3>(
311 std::bind(&factorErrorWithTransform3D, std::placeholders::_1,
point,
factor),
pose);
312 H2Expected = numericalDerivative11<Vector, Point3>(
313 std::bind(&factorErrorWithTransform3D,
pose, std::placeholders::_1,
factor),
point);
330 Vector expectedError = (
Vector(1) << 0.295630141).finished();
344 Point3 p11(1.0, 2.0, 0.0),
p22(-4.0, 11.0, 0);
347 Vector expectedError = (
Vector(1) << 0.295630141).finished();
373 return (
v2 -
v1).norm();
387 Vector4
pose(1.0, 2.0, 00, 0);
388 Vector4
point(-4.0, 11.0, 0, 0);
391 Vector expectedError = (
Vector(1) << 0.295630141).finished();
394 CHECK(
assert_equal(expectedError,
factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}), 1
e-9));