testVelocityConstraint.cpp
Go to the documentation of this file.
1 
7 
9 
10 using namespace gtsam;
11 
12 const double tol=1e-5;
13 
14 const Key x1 = 1, x2 = 2;
15 const double dt = 1.0;
16 
18  pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), Velocity3(1.0, 0.0, 0.0)),
19  pose1a(Point3(0.5, 0.0, 0.0)),
20  pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), Velocity3(1.0, 0.0, 0.0));
21 
22 /* ************************************************************************* */
23 TEST( testVelocityConstraint, trapezoidal ) {
24  // hard constraints don't need a noise model
26 
27  // verify error function
28  EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, pose1), tol));
29  EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, origin), tol));
30  EXPECT(assert_equal(Vector::Unit(3,0)*(-1.0), constraint.evaluateError(pose1, pose1), tol));
31  EXPECT(assert_equal(Vector::Unit(3,0)*0.5, constraint.evaluateError(origin, pose1a), tol));
32 }
33 
34 /* ************************************************************************* */
35 TEST( testEulerVelocityConstraint, euler_start ) {
36  // hard constraints create their own noise model
38 
39  // verify error function
40  EXPECT(assert_equal(Vector::Unit(3,0)*0.5, constraint.evaluateError(origin, pose1), tol));
41  EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, origin), tol));
43  EXPECT(assert_equal(Vector::Unit(3,0)*0.5, constraint.evaluateError(origin, pose1a), tol));
44 }
45 
46 /* ************************************************************************* */
47 TEST( testEulerVelocityConstraint, euler_end ) {
48  // hard constraints create their own noise model
50 
51  // verify error function
52  EXPECT(assert_equal(Vector::Unit(3,0)*(-0.5), constraint.evaluateError(origin, pose1), tol));
53  EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, origin), tol));
55  EXPECT(assert_equal(Vector::Unit(3,0)*0.5, constraint.evaluateError(origin, pose1a), tol));
56 }
57 
58 /* ************************************************************************* */
59 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
60 /* ************************************************************************* */
PoseRTV pose1a(Point3(0.5, 0.0, 0.0))
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:46
static int runAllTests(TestResult &result)
Vector3 Velocity3
Velocity is currently typedef&#39;d to Vector3.
Definition: NavState.h:28
const Key x2
Constraint enforcing the relationship between pose and velocity.
const double dt
static Rot3 identity()
identity rotation for group operation
Definition: Rot3.h:303
int main()
#define EXPECT(condition)
Definition: Test.h:151
Array< double, 1, 3 > e(1./3., 0.5, 2.)
PoseRTV pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), Velocity3(1.0, 0.0, 0.0))
traits
Definition: chartTesting.h:28
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
const Key x1
gtsam::Vector evaluateError(const PoseRTV &x1, const PoseRTV &x2, boost::optional< gtsam::Matrix & > H1=boost::none, boost::optional< gtsam::Matrix & > H2=boost::none) const override
TEST(LPInitSolver, InfiniteLoopSingleVar)
const G double tol
Definition: Group.h:83
Vector3 Point3
Definition: Point3.h:35
PoseRTV pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), Velocity3(1.0, 0.0, 0.0))
PoseRTV origin
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:50:25