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 #include <boost/bind.hpp>
29 #include <boost/format.hpp>
30 
31 using namespace std;
32 using namespace gtsam;
33 
34 // typedefs
37 
38 // units
39 static const double ms = 1e-3;
40 static const double cm = 1e-2;
41 
42 // Create a noise model for the TOA error
43 static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms));
44 
45 static const double timeOfEvent = 25;
46 static const Event exampleEvent(timeOfEvent, 1, 0, 0);
47 static const Point3 sensorAt0(0, 0, 0);
48 
49 //*****************************************************************************
50 TEST(TOAFactor, NewWay) {
51  Key key = 12;
52  double measurement = 7;
53  TOAFactor factor(key, sensorAt0, measurement, model);
54 }
55 
56 //*****************************************************************************
57 TEST(TOAFactor, WholeEnchilada) {
58  // Create sensors
59  const double height = 0.5;
60  vector<Point3> sensors;
61  sensors.push_back(Point3(0, 0, height));
62  sensors.push_back(Point3(403 * cm, 0, height));
63  sensors.push_back(Point3(403 * cm, 403 * cm, height));
64  sensors.push_back(Point3(0, 403 * cm, 2 * height));
65  EXPECT_LONGS_EQUAL(4, sensors.size());
66  // sensors.push_back(Point3(200 * cm, 200 * cm, height));
67 
68  // Create a ground truth point
69  const double timeOfEvent = 0;
70  Event groundTruthEvent(timeOfEvent, 245 * cm, 201.5 * cm, (212 - 45) * cm);
71 
72  // Simulate simulatedTOA
73  size_t K = sensors.size();
74  vector<double> simulatedTOA(K);
75  TimeOfArrival toa;
76  for (size_t i = 0; i < K; i++) {
77  simulatedTOA[i] = toa(groundTruthEvent, sensors[i]);
78  }
79 
80  // Now, estimate using non-linear optimization
82  Key key = 12;
83  for (size_t i = 0; i < K; i++) {
84  graph.emplace_shared<TOAFactor>(key, sensors[i], simulatedTOA[i], model);
85  }
86 
87  // Create initial estimate
88  Values initialEstimate;
89  // Event estimatedEvent(timeOfEvent -10, 200 * cm, 150 * cm, 350 * cm);
90  Vector4 delta;
91  delta << 0.1, 0.1, -0.1, 0.1;
92  Event estimatedEvent = groundTruthEvent.retract(delta);
93  initialEstimate.insert(key, estimatedEvent);
94 
95  // Optimize using Levenberg-Marquardt optimization.
97  params.setAbsoluteErrorTol(1e-10);
98  LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params);
99  Values result = optimizer.optimize();
100 
101  EXPECT(assert_equal(groundTruthEvent, result.at<Event>(key), 1e-6));
102 }
103 //*****************************************************************************
104 int main() {
105  TestResult tr;
106  return TestRegistry::runAllTests(tr);
107 }
108 //*****************************************************************************
virtual const Values & optimize()
int main()
static int runAllTests(TestResult &result)
Event retract(const Vector4 &v) const
Updates a with tangent space delta.
Definition: Event.h:72
Factor Graph consisting of non-linear factors.
static const double timeOfEvent
"Time of Arrival" factor
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
double measurement(10.0)
Definition: Half.h:150
Some functions to compute numerical derivatives.
NonlinearFactorGraph graph
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&...args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:172
Expression< Event > Event_
Values result
const ValueType at(Key j) const
Definition: Values-inl.h:342
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:151
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static SmartStereoProjectionParams params
static const double cm
Expression< Point3 > Point3_
traits
Definition: chartTesting.h:28
Time of arrival to given sensor.
Definition: Event.h:87
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
static const Point3 sensorAt0(0, 0, 0)
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:155
static const Event exampleEvent(timeOfEvent, 1, 0, 0)
Vector3 Point3
Definition: Point3.h:35
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 *ms))
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
TEST(TOAFactor, NewWay)
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:734


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