Event header and test file
dellaert
parent
5d6e0bc753
commit
d17caa5487
106
.cproject
106
.cproject
|
|
@ -542,6 +542,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testEvent.run" path="build/gtsam_unstable/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testEvent.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -592,6 +600,7 @@
|
|||
</target>
|
||||
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>tests/testBayesTree.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -599,6 +608,7 @@
|
|||
</target>
|
||||
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testBinaryBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -646,6 +656,7 @@
|
|||
</target>
|
||||
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -653,6 +664,7 @@
|
|||
</target>
|
||||
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>tests/testSymbolicFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -660,6 +672,7 @@
|
|||
</target>
|
||||
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -675,6 +688,7 @@
|
|||
</target>
|
||||
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>tests/testBayesTree</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1138,6 +1152,7 @@
|
|||
</target>
|
||||
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testErrors.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1367,6 +1382,46 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testBTree.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testDSF.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testDSFMap.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testDSFVector.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testFixedVector.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -1449,7 +1504,6 @@
|
|||
</target>
|
||||
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSimulated2DOriented.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1489,7 +1543,6 @@
|
|||
</target>
|
||||
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSimulated2D.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1497,7 +1550,6 @@
|
|||
</target>
|
||||
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSimulated3D.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1511,46 +1563,6 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testBTree.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testDSF.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testDSFMap.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testDSFVector.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testFixedVector.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
@ -1808,6 +1820,7 @@
|
|||
</target>
|
||||
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>cpack</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>-G DEB</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1815,6 +1828,7 @@
|
|||
</target>
|
||||
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>cpack</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>-G RPM</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1822,6 +1836,7 @@
|
|||
</target>
|
||||
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>cpack</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>-G TGZ</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1829,6 +1844,7 @@
|
|||
</target>
|
||||
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>cpack</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -2683,6 +2699,7 @@
|
|||
</target>
|
||||
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -2690,6 +2707,7 @@
|
|||
</target>
|
||||
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testJunctionTree.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -2697,6 +2715,7 @@
|
|||
</target>
|
||||
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicBayesNetB.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -3248,7 +3267,6 @@
|
|||
</target>
|
||||
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>tests/testGaussianISAM2</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
|
|||
|
|
@ -0,0 +1,115 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 Event
|
||||
* @brief Space-time event
|
||||
* @author Frank Dellaert
|
||||
* @author Jay Chakravarty
|
||||
* @date December 2014
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <cmath>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/// A space-time event
|
||||
class Event {
|
||||
|
||||
double time_; ///< Time event was generated
|
||||
Point3 location_; ///< Location at time event was generated
|
||||
|
||||
public:
|
||||
|
||||
/// Speed of sound
|
||||
static const double Speed;
|
||||
|
||||
/// Default Constructor
|
||||
Event() :
|
||||
time_(0) {
|
||||
}
|
||||
|
||||
/// Constructor from time and location
|
||||
Event(double t, const Point3& p) :
|
||||
time_(t), location_(p) {
|
||||
}
|
||||
|
||||
/// Constructor with doubles
|
||||
Event(double t, double x, double y, double z) :
|
||||
time_(t), location_(x, y, z) {
|
||||
}
|
||||
|
||||
/** print with optional string */
|
||||
void print(const std::string& s = "") const {
|
||||
std::cout << s << "time = " << time_;
|
||||
location_.print("; location");
|
||||
}
|
||||
|
||||
/** equals with an tolerance */
|
||||
bool equals(const Event& other, double tol = 1e-9) const {
|
||||
return std::abs(time_ - other.time_) < tol
|
||||
&& location_.equals(other.location_, tol);
|
||||
}
|
||||
|
||||
/// Manifold stuff:
|
||||
|
||||
size_t dim() const {
|
||||
return 4;
|
||||
}
|
||||
static size_t Dim() {
|
||||
return 4;
|
||||
}
|
||||
|
||||
/// Updates a with tangent space delta
|
||||
inline Event retract(const Vector4& v) const {
|
||||
return Event(time_ + v[0], location_.retract(v.tail(3)));
|
||||
}
|
||||
|
||||
/// Returns inverse retraction
|
||||
inline Vector4 localCoordinates(const Event& q) const {
|
||||
return Vector4::Zero(); // TODO
|
||||
}
|
||||
|
||||
/// Time of arrival to given microphone
|
||||
double toa(const Point3& microphone, //
|
||||
OptionalJacobian<1, 4> H1 = boost::none, //
|
||||
OptionalJacobian<1, 3> H2 = boost::none) const {
|
||||
Matrix13 D1, D2;
|
||||
double distance = location_.distance(microphone, D1, D2);
|
||||
if (H1)
|
||||
// derivative of toa with respect to event
|
||||
*H1 << 1.0, D1 / Speed;
|
||||
if (H2)
|
||||
// derivative of toa with respect to microphone location
|
||||
*H2 << D2 / Speed;
|
||||
return time_ + distance / Speed;
|
||||
}
|
||||
};
|
||||
|
||||
const double Event::Speed = 330;
|
||||
|
||||
// Define GTSAM traits
|
||||
namespace traits {
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT dimension<Event> : public boost::integral_constant<int, 4> {
|
||||
};
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT is_manifold<Event> : public boost::true_type {
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
} //\ namespace gtsam
|
||||
|
|
@ -0,0 +1,100 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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);
|
||||
}
|
||||
//*****************************************************************************
|
||||
|
||||
|
|
@ -17,102 +17,12 @@
|
|||
* @date December 2014
|
||||
*/
|
||||
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam_unstable/nonlinear/ExpressionFactor.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <cmath>
|
||||
#include <gtsam_unstable/geometry/Event.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/// A space-time event
|
||||
class Event {
|
||||
|
||||
double time_; ///< Time event was generated
|
||||
Point3 location_; ///< Location at time event was generated
|
||||
|
||||
public:
|
||||
|
||||
/// Speed of sound
|
||||
static const double Speed;
|
||||
|
||||
/// Default Constructor
|
||||
Event() :
|
||||
time_(0) {
|
||||
}
|
||||
|
||||
/// Constructor from time and location
|
||||
Event(double t, const Point3& p) :
|
||||
time_(t), location_(p) {
|
||||
}
|
||||
|
||||
/// Constructor with doubles
|
||||
Event(double t, double x, double y, double z) :
|
||||
time_(t), location_(x, y, z) {
|
||||
}
|
||||
|
||||
/** print with optional string */
|
||||
void print(const std::string& s = "") const {
|
||||
std::cout << s << "time = " << time_;
|
||||
location_.print("; location");
|
||||
}
|
||||
|
||||
/** equals with an tolerance */
|
||||
bool equals(const Event& other, double tol = 1e-9) const {
|
||||
return std::abs(time_ - other.time_) < tol
|
||||
&& location_.equals(other.location_, tol);
|
||||
}
|
||||
|
||||
/// Manifold stuff:
|
||||
|
||||
size_t dim() const {
|
||||
return 4;
|
||||
}
|
||||
static size_t Dim() {
|
||||
return 4;
|
||||
}
|
||||
|
||||
/// Updates a with tangent space delta
|
||||
inline Event retract(const Vector4& v) const {
|
||||
return Event(time_ + v[0], location_.retract(v.tail(3)));
|
||||
}
|
||||
|
||||
/// Returns inverse retraction
|
||||
inline Vector4 localCoordinates(const Event& q) const {
|
||||
return Vector4::Zero(); // TODO
|
||||
}
|
||||
|
||||
/// Time of arrival to given microphone
|
||||
double toa(const Point3& microphone, //
|
||||
OptionalJacobian<1, 4> H1 = boost::none, //
|
||||
OptionalJacobian<1, 3> H2 = boost::none) const {
|
||||
Matrix13 D1, D2;
|
||||
double distance = location_.distance(microphone, D1, D2);
|
||||
if (H1)
|
||||
// derivative of toa with respect to event
|
||||
*H1 << 1.0, D1 / Speed;
|
||||
if (H2)
|
||||
// derivative of toa with respect to microphone location
|
||||
*H2 << D2 / Speed;
|
||||
return time_ + distance / Speed;
|
||||
}
|
||||
};
|
||||
|
||||
const double Event::Speed = 330;
|
||||
|
||||
// Define GTSAM traits
|
||||
namespace traits {
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT dimension<Event> : public boost::integral_constant<int, 4> {
|
||||
};
|
||||
|
||||
template<>
|
||||
struct GTSAM_EXPORT is_manifold<Event> : public boost::true_type {
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
/// A "Time of Arrival" factor
|
||||
/// A "Time of Arrival" factor - so little code seems hardly worth it :-)
|
||||
class TOAFactor: public ExpressionFactor<double> {
|
||||
|
||||
typedef Expression<double> double_;
|
||||
|
|
@ -137,6 +47,7 @@ public:
|
|||
|
||||
} //\ namespace gtsam
|
||||
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
|
@ -155,61 +66,6 @@ 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)));
|
||||
}
|
||||
|
||||
//*****************************************************************************
|
||||
TEST( TOAFactor, Construct ) {
|
||||
Key key = 12;
|
||||
|
|
|
|||
Loading…
Reference in New Issue