101 lines
3.3 KiB
C++
101 lines
3.3 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
* Atlanta, Georgia 30332-0415
|
|
* All Rights Reserved
|
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
|
|
* See LICENSE for the license information
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file testEvent.cpp
|
|
* @brief Unit tests for space time "Event"
|
|
* @author Frank Dellaert
|
|
* @author Jay Chakravarty
|
|
* @date December 2014
|
|
*/
|
|
|
|
#include <gtsam_unstable/geometry/Event.h>
|
|
#include <gtsam/base/numericalDerivative.h>
|
|
#include <gtsam/nonlinear/Expression.h>
|
|
#include <CppUnitLite/TestHarness.h>
|
|
#include <boost/bind.hpp>
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
|
|
// Create a noise model for the TOA error
|
|
static const double ms = 1e-3;
|
|
static const double cm = 1e-2;
|
|
typedef Eigen::Matrix<double, 1, 1> Vector1;
|
|
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1,0.5*ms));
|
|
|
|
static const double timeOfEvent = 25;
|
|
static const Event exampleEvent(timeOfEvent, 1, 0, 0);
|
|
static const Point3 microphoneAt0;
|
|
|
|
//*****************************************************************************
|
|
TEST( Event, Constructor ) {
|
|
const double t = 0;
|
|
Event actual(t, 201.5 * cm, 201.5 * cm, (212 - 45) * cm);
|
|
}
|
|
|
|
//*****************************************************************************
|
|
TEST( Event, Toa1 ) {
|
|
Event event(0, 1, 0, 0);
|
|
double expected = 1 / Event::Speed;
|
|
EXPECT_DOUBLES_EQUAL(expected, event.toa(microphoneAt0), 1e-9);
|
|
}
|
|
|
|
//*****************************************************************************
|
|
TEST( Event, Toa2 ) {
|
|
double expectedTOA = timeOfEvent + 1 / Event::Speed;
|
|
EXPECT_DOUBLES_EQUAL(expectedTOA, exampleEvent.toa(microphoneAt0), 1e-9);
|
|
}
|
|
|
|
//*************************************************************************
|
|
TEST (Event, Derivatives) {
|
|
Matrix14 actualH1;
|
|
Matrix13 actualH2;
|
|
exampleEvent.toa(microphoneAt0, actualH1, actualH2);
|
|
Matrix expectedH1 = numericalDerivative11<double, Event>(
|
|
boost::bind(&Event::toa, _1, microphoneAt0, boost::none, boost::none),
|
|
exampleEvent);
|
|
EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
|
|
Matrix expectedH2 = numericalDerivative11<double, Point3>(
|
|
boost::bind(&Event::toa, exampleEvent, _1, boost::none, boost::none),
|
|
microphoneAt0);
|
|
EXPECT(assert_equal(expectedH2, actualH2, 1e-8));
|
|
}
|
|
|
|
//*****************************************************************************
|
|
TEST( Event, Expression ) {
|
|
Key key = 12;
|
|
Expression<Event> event_(key);
|
|
Expression<Point3> knownMicrophone_(microphoneAt0); // constant expression
|
|
Expression<double> expression(&Event::toa, event_, knownMicrophone_);
|
|
|
|
Values values;
|
|
values.insert(key, exampleEvent);
|
|
double expectedTOA = timeOfEvent + 1 / Event::Speed;
|
|
EXPECT_DOUBLES_EQUAL(expectedTOA, expression.value(values), 1e-9);
|
|
}
|
|
|
|
//*****************************************************************************
|
|
TEST(Event, Retract) {
|
|
Event event, expected(1, 2, 3, 4);
|
|
Vector4 v;
|
|
v << 1, 2, 3, 4;
|
|
EXPECT(assert_equal(expected, event.retract(v)));
|
|
}
|
|
|
|
//*****************************************************************************
|
|
int main() {
|
|
TestResult tr;
|
|
return TestRegistry::runAllTests(tr);
|
|
}
|
|
//*****************************************************************************
|
|
|