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
12/**
13 * @file testTOAFactor.cpp
14 * @brief Unit tests for "Time of Arrival" factor
15 * @author Frank Dellaert
16 * @author Jay Chakravarty
17 * @date December 2014
18 */
19
20#include <gtsam/base/numericalDerivative.h>
21#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
22#include <gtsam/nonlinear/NonlinearFactorGraph.h>
23#include <gtsam/nonlinear/expressions.h>
24#include <gtsam/geometry/Event.h>
25#include <gtsam_unstable/slam/TOAFactor.h>
26
27#include <CppUnitLite/TestHarness.h>
28
29using namespace std;
30using namespace gtsam;
31
32// typedefs
33typedef Expression<Point3> Point3_;
34typedef Expression<Event> Event_;
35
36// units
37static const double ms = 1e-3;
38static const double cm = 1e-2;
39
40// Create a noise model for the TOA error
41static SharedNoiseModel model(noiseModel::Isotropic::Sigma(dim: 1, sigma: 0.5 * ms));
42
43static const double timeOfEvent = 25;
44static const Event exampleEvent(timeOfEvent, 1, 0, 0);
45static const Point3 sensorAt0(0, 0, 0);
46
47//*****************************************************************************
48TEST(TOAFactor, NewWay) {
49 Key key = 12;
50 double measurement = 7;
51 TOAFactor factor(key, sensorAt0, measurement, model);
52}
53
54//*****************************************************************************
55TEST(TOAFactor, WholeEnchilada) {
56 // Create sensors
57 const double height = 0.5;
58 vector<Point3> sensors;
59 sensors.push_back(x: Point3(0, 0, height));
60 sensors.push_back(x: Point3(403 * cm, 0, height));
61 sensors.push_back(x: Point3(403 * cm, 403 * cm, height));
62 sensors.push_back(x: Point3(0, 403 * cm, 2 * height));
63 EXPECT_LONGS_EQUAL(4, sensors.size());
64 // sensors.push_back(Point3(200 * cm, 200 * cm, height));
65
66 // Create a ground truth point
67 const double timeOfEvent = 0;
68 Event groundTruthEvent(timeOfEvent, 245 * cm, 201.5 * cm, (212 - 45) * cm);
69
70 // Simulate simulatedTOA
71 size_t K = sensors.size();
72 vector<double> simulatedTOA(K);
73 TimeOfArrival toa;
74 for (size_t i = 0; i < K; i++) {
75 simulatedTOA[i] = toa(groundTruthEvent, sensors[i]);
76 }
77
78 // Now, estimate using non-linear optimization
79 NonlinearFactorGraph graph;
80 Key key = 12;
81 for (size_t i = 0; i < K; i++) {
82 graph.emplace_shared<TOAFactor>(args&: key, args&: sensors[i], args&: simulatedTOA[i], args&: model);
83 }
84
85 // Create initial estimate
86 Values initialEstimate;
87 // Event estimatedEvent(timeOfEvent -10, 200 * cm, 150 * cm, 350 * cm);
88 Vector4 delta;
89 delta << 0.1, 0.1, -0.1, 0.1;
90 Event estimatedEvent = groundTruthEvent.retract(v: delta);
91 initialEstimate.insert(j: key, val: estimatedEvent);
92
93 // Optimize using Levenberg-Marquardt optimization.
94 LevenbergMarquardtParams params;
95 params.setAbsoluteErrorTol(1e-10);
96 LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params);
97 Values result = optimizer.optimize();
98
99 EXPECT(assert_equal(groundTruthEvent, result.at<Event>(key), 1e-6));
100}
101//*****************************************************************************
102int main() {
103 TestResult tr;
104 return TestRegistry::runAllTests(result&: tr);
105}
106//*****************************************************************************
107