1/**
2 * @file testVelocityConstraint3
3 * @author Duy-Nguyen Ta
4 */
5
6#include <CppUnitLite/TestHarness.h>
7
8#include <gtsam/inference/Symbol.h>
9#include <gtsam_unstable/dynamics/VelocityConstraint3.h>
10
11using namespace gtsam;
12using namespace gtsam::symbol_shorthand;
13
14/* ************************************************************************* */
15// evaluateError
16TEST( testVelocityConstraint3, evaluateError) {
17
18 const double tol = 1e-5;
19 const double dt = 1.0;
20 double x1(1.0), x2(2.0), v(1.0);
21
22 // hard constraints don't need a noise model
23 VelocityConstraint3 constraint(X(j: 1), X(j: 2), V(j: 1), dt);
24
25 // verify error function
26 EXPECT(assert_equal(Z_1x1, constraint.evaluateError(x1, x2, v), tol));
27}
28
29/* ************************************************************************* */
30int main() {
31 TestResult tr;
32 return TestRegistry::runAllTests(result&: tr);
33}
34/* ************************************************************************* */
35