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(), 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(), 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
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));
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));
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 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::dynamics::EULER_END
@ EULER_END
Definition: VelocityConstraint.h:21
origin
PoseRTV origin
Definition: testVelocityConstraint.cpp:17
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::Z_3x1
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:46
TestHarness.h
gtsam::Velocity3
Vector3 Velocity3
Velocity is currently typedef'd to Vector3.
Definition: NavState.h:28
dt
const double dt
Definition: testVelocityConstraint.cpp:15
x2
const Key x2
Definition: testVelocityConstraint.cpp:14
main
int main()
Definition: testVelocityConstraint.cpp:59
pose2
PoseRTV pose2(Point3(1.5, 0.0, 0.0), Rot3(), Velocity3(1.0, 0.0, 0.0))
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
TestResult
Definition: TestResult.h:26
x1
const Key x1
Definition: testVelocityConstraint.cpp:14
gtsam::PoseRTV
Definition: PoseRTV.h:23
gtsam
traits
Definition: SFMdata.h:40
gtsam::TEST
TEST(SmartFactorBase, Pinhole)
Definition: testSmartFactorBase.cpp:38
gtsam::dynamics::TRAPEZOIDAL
@ TRAPEZOIDAL
Definition: VelocityConstraint.h:19
gtsam::VelocityConstraint
Definition: VelocityConstraint.h:33
pose1
PoseRTV pose1(Point3(0.5, 0.0, 0.0), Rot3(), Velocity3(1.0, 0.0, 0.0))
gtsam::VelocityConstraint::evaluateError
gtsam::Vector evaluateError(const PoseRTV &x1, const PoseRTV &x2, OptionalMatrixType H1, OptionalMatrixType H2) const override
Definition: VelocityConstraint.h:86
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
VelocityConstraint.h
Constraint enforcing the relationship between pose and velocity.
pose1a
PoseRTV pose1a(Point3(0.5, 0.0, 0.0))
gtsam::dynamics::EULER_START
@ EULER_START
Definition: VelocityConstraint.h:20
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:42:03