testTOAFactor.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 
26 
28 
29 using namespace std;
30 using namespace gtsam;
31 
32 // typedefs
35 
36 // units
37 static const double ms = 1e-3;
38 static const double cm = 1e-2;
39 
40 // Create a noise model for the TOA error
41 static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms));
42 
43 static const double timeOfEvent = 25;
44 static const Event exampleEvent(timeOfEvent, 1, 0, 0);
45 static const Point3 sensorAt0(0, 0, 0);
46 
47 //*****************************************************************************
48 TEST(TOAFactor, NewWay) {
49  Key key = 12;
50  double measurement = 7;
51  TOAFactor factor(key, sensorAt0, measurement, model);
52 }
53 
54 //*****************************************************************************
55 TEST(TOAFactor, WholeEnchilada) {
56  // Create sensors
57  const double height = 0.5;
58  vector<Point3> sensors;
59  sensors.push_back(Point3(0, 0, height));
60  sensors.push_back(Point3(403 * cm, 0, height));
61  sensors.push_back(Point3(403 * cm, 403 * cm, height));
62  sensors.push_back(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
80  Key key = 12;
81  for (size_t i = 0; i < K; i++) {
82  graph.emplace_shared<TOAFactor>(key, sensors[i], simulatedTOA[i], 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(delta);
91  initialEstimate.insert(key, estimatedEvent);
92 
93  // Optimize using Levenberg-Marquardt optimization.
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 //*****************************************************************************
102 int main() {
103  TestResult tr;
104  return TestRegistry::runAllTests(tr);
105 }
106 //*****************************************************************************
const gtsam::Symbol key('X', 0)
virtual const Values & optimize()
int main()
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:196
static int runAllTests(TestResult &result)
Factor Graph consisting of non-linear factors.
static const double timeOfEvent
const ValueType at(Key j) const
Definition: Values-inl.h:204
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
"Time of Arrival" factor
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
Definition: BFloat16.h:88
Some functions to compute numerical derivatives.
Event retract(const Vector4 &v) const
Updates a with tangent space delta.
Definition: Event.h:72
NonlinearFactorGraph graph
static const SmartProjectionParams params
static Point2 measurement(323.0, 240.0)
Expression< Event > Event_
Values result
static const double ms
A "Time of Arrival" factor - so little code seems hardly worth it :-)
Definition: TOAFactor.h:28
#define EXPECT(condition)
Definition: Test.h:150
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static const double cm
Expression< Point3 > Point3_
traits
Definition: chartTesting.h:28
Time of arrival to given sensor.
Definition: Event.h:87
static const Point3 sensorAt0(0, 0, 0)
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
Space-time event.
static const Event exampleEvent(timeOfEvent, 1, 0, 0)
Vector3 Point3
Definition: Point3.h:38
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 *ms))
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
TEST(TOAFactor, NewWay)
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741


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