testEvent.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
23 
25 
26 using namespace std::placeholders;
27 using namespace std;
28 using namespace gtsam;
29 
30 // Create a noise model for the TOA error
31 static const double ms = 1e-3;
32 static const double cm = 1e-2;
34 static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms));
35 
36 static const double timeOfEvent = 25;
37 static const Event exampleEvent(timeOfEvent, 1, 0, 0);
38 static const Point3 microphoneAt0(0, 0, 0);
39 
40 static const double kSpeedOfSound = 340;
41 static const TimeOfArrival kToa(kSpeedOfSound);
42 
43 //*****************************************************************************
44 TEST(Event, Constructor) {
45  const double t = 0;
46  Event actual(t, 201.5 * cm, 201.5 * cm, (212 - 45) * cm);
47 }
48 
49 //*****************************************************************************
50 TEST(Event, Toa1) {
51  Event event(0, 1, 0, 0);
52  double expected = 1. / kSpeedOfSound;
54 }
55 
56 //*****************************************************************************
57 TEST(Event, Toa2) {
58  double expectedTOA = timeOfEvent + 1. / kSpeedOfSound;
60 }
61 
62 //*************************************************************************
63 TEST(Event, Derivatives) {
64  Matrix14 actualH1;
65  Matrix13 actualH2;
66  kToa(exampleEvent, microphoneAt0, actualH1, actualH2);
67  Matrix expectedH1 = numericalDerivative11<double, Event>(
68  std::bind(kToa, std::placeholders::_1, microphoneAt0, nullptr, nullptr),
69  exampleEvent);
70  EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
71  Matrix expectedH2 = numericalDerivative11<double, Point3>(
72  std::bind(kToa, exampleEvent, std::placeholders::_1, nullptr, nullptr),
74  EXPECT(assert_equal(expectedH2, actualH2, 1e-8));
75 }
76 
77 //*****************************************************************************
79  Key key = 12;
80  Expression<Event> event_(key);
81  Expression<Point3> knownMicrophone_(microphoneAt0); // constant expression
82  Expression<double> expression(kToa, event_, knownMicrophone_);
83 
84  Values values;
86  double expectedTOA = timeOfEvent + 1. / kSpeedOfSound;
87  EXPECT_DOUBLES_EQUAL(expectedTOA, expression.value(values), 1e-9);
88 }
89 
90 //*****************************************************************************
91 TEST(Event, Retract) {
92  Event event, expected(1, 2, 3, 4);
93  Vector4 v;
94  v << 1, 2, 3, 4;
96 }
97 
98 //*****************************************************************************
99 int main() {
100  TestResult tr;
101  return TestRegistry::runAllTests(tr);
102 }
103 //*****************************************************************************
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
main
int main()
Definition: testEvent.cpp:99
model
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 *ms))
gtsam::Expression::value
T value(const Values &values, std::vector< Matrix > *H=nullptr) const
Return value and optional derivatives, reverse AD version Notes: this is not terribly efficient,...
Definition: Expression-inl.h:142
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
TEST
TEST(Event, Constructor)
Definition: testEvent.cpp:44
ms
static const double ms
Definition: testEvent.cpp:31
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
kSpeedOfSound
static const double kSpeedOfSound
Definition: testEvent.cpp:40
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::TimeOfArrival
Time of arrival to given sensor.
Definition: Event.h:87
timeOfEvent
static const double timeOfEvent
Definition: testEvent.cpp:36
gtsam::Expression
Definition: Expression.h:47
Vector1
Eigen::Matrix< double, 1, 1 > Vector1
Definition: testEvent.cpp:33
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::Event::retract
Event retract(const Vector4 &v) const
Updates a with tangent space delta.
Definition: Event.h:72
cm
static const double cm
Definition: testEvent.cpp:32
microphoneAt0
static const Point3 microphoneAt0(0, 0, 0)
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Definition: HybridValues.cpp:85
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
Event.h
Space-time event.
gtsam::Event
Definition: Event.h:37
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
TestResult
Definition: TestResult.h:26
key
const gtsam::Symbol key('X', 0)
gtsam
traits
Definition: SFMdata.h:40
kToa
static const TimeOfArrival kToa(kSpeedOfSound)
gtsam::Values
Definition: Values.h:65
std
Definition: BFloat16.h:88
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
align_3::t
Point2 t(10, 10)
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
Expression.h
Expressions for Block Automatic Differentiation.
exampleEvent
static const Event exampleEvent(timeOfEvent, 1, 0, 0)


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