testVelocityConstraint3.cpp
Go to the documentation of this file.
1 
7 
10 
11 using namespace gtsam;
12 using namespace gtsam::symbol_shorthand;
13 
14 /* ************************************************************************* */
15 // evaluateError
16 TEST( 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(1), X(2), V(1), dt);
24 
25  // verify error function
26  EXPECT(assert_equal(Z_1x1, constraint.evaluateError(x1, x2, v), tol));
27 }
28 
29 /* ************************************************************************* */
30 int main() {
31  TestResult tr;
32  return TestRegistry::runAllTests(tr);
33 }
34 /* ************************************************************************* */
static int runAllTests(TestResult &result)
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
Key X(std::uint64_t j)
Vector evaluateError(const double &x1, const double &x2, const double &v, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override
const double dt
#define EXPECT(condition)
Definition: Test.h:150
Array< int, Dynamic, 1 > v
Array< double, 1, 3 > e(1./3., 0.5, 2.)
traits
Definition: chartTesting.h:28
Pose3 x1
Definition: testPose3.cpp:663
Key V(std::uint64_t j)
const G double tol
Definition: Group.h:86
TEST(SmartFactorBase, Pinhole)
A simple 3-way factor constraining double poses and velocity.


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:39:57