12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Now it's super-easy to create priors on pieces of state.

release/4.3a0
dellaert 2014-12-11 13:45:15 +01:00
parent 31ca0d76db
commit a9121fc3fc
2 changed files with 25 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam_unstable/geometry/Event.h
View File

@ -34,6 +34,7 @@ public:
/// Speed of sound /// Speed of sound
static const double Speed; static const double Speed;
static const Matrix14 JacobianZ;
/// Default Constructor /// Default Constructor
Event() : Event() :
@ -53,6 +54,12 @@ public:
double time() const { return time_;} double time() const { return time_;}
Point3 location() const { return location_;} Point3 location() const { return location_;}
// TODO we really have to think of a better way to do linear arguments
double height(OptionalJacobian<1,4> H = boost::none) const {
if (H) *H = JacobianZ;
return location_.z();
}
/** print with optional string */ /** print with optional string */
void print(const std::string& s = "") const { void print(const std::string& s = "") const {
std::cout << s << "time = " << time_; std::cout << s << "time = " << time_;
@ -101,6 +108,7 @@ public:
}; };
const double Event::Speed = 330; const double Event::Speed = 330;
const Matrix14 Event::JacobianZ = (Matrix14() << 0,0,0,1).finished();
// Define GTSAM traits // Define GTSAM traits
namespace traits { namespace traits {

26
gtsam_unstable/slam/tests/testTOAFactor.cpp
View File

@ -40,8 +40,12 @@ static const double timeOfEvent = 25;
static const Event exampleEvent(timeOfEvent, 1, 0, 0); static const Event exampleEvent(timeOfEvent, 1, 0, 0);
static const Point3 microphoneAt0; static const Point3 microphoneAt0;
// A TOA factor factory :-) // A TOA factor factory
MakeBinaryFactor<double, Event, Point3> makeFactor(&Event::toa, model); static MakeBinaryFactor<double, Event, Point3> MakeFactor(&Event::toa, model);
// A height prior factory
static SharedNoiseModel heightModel(noiseModel::Isotropic::Sigma(1, 100*cm));
static MakeUnaryFactor<double, Event> MakePrior(&Event::height, heightModel);
//***************************************************************************** //*****************************************************************************
TEST( TOAFactor, NewWay ) { TEST( TOAFactor, NewWay ) {
@ -49,7 +53,7 @@ TEST( TOAFactor, NewWay ) {
Expression<Event> eventExpression(key); Expression<Event> eventExpression(key);
Expression<Point3> microphoneConstant(microphoneAt0); // constant expression Expression<Point3> microphoneConstant(microphoneAt0); // constant expression
double z = 7; double z = 7;
ExpressionFactor<double> factor = makeFactor(z, eventExpression, microphoneConstant); ExpressionFactor<double> factor = MakeFactor(z, eventExpression, microphoneConstant);
} }
//***************************************************************************** //*****************************************************************************
@ -63,9 +67,9 @@ TEST( TOAFactor, WholeEnchilada ) {
microphones.push_back(Point3(0, 0, height)); microphones.push_back(Point3(0, 0, height));
microphones.push_back(Point3(403 * cm, 0, height)); microphones.push_back(Point3(403 * cm, 0, height));
microphones.push_back(Point3(403 * cm, 403 * cm, height)); microphones.push_back(Point3(403 * cm, 403 * cm, height));
microphones.push_back(Point3(0, 403 * cm, height)); microphones.push_back(Point3(0, 403 * cm, 2*height));
EXPECT_LONGS_EQUAL(4, microphones.size()); EXPECT_LONGS_EQUAL(4, microphones.size());
microphones.push_back(Point3(200 * cm, 200 * cm, height)); // microphones.push_back(Point3(200 * cm, 200 * cm, height));
// Create a ground truth point // Create a ground truth point
const double timeOfEvent = 0; const double timeOfEvent = 0;
@ -88,7 +92,7 @@ TEST( TOAFactor, WholeEnchilada ) {
Expression<Event> eventExpression(key); Expression<Event> eventExpression(key);
for (size_t i = 0; i < K; i++) { for (size_t i = 0; i < K; i++) {
Expression<Point3> microphoneConstant(microphones[i]); // constant expression Expression<Point3> microphoneConstant(microphones[i]); // constant expression
graph.add(makeFactor(measurements[i], eventExpression, microphoneConstant)); graph.add(MakeFactor(measurements[i], eventExpression, microphoneConstant));
} }
/// Print the graph /// Print the graph
@ -123,7 +127,7 @@ TEST( TOAFactor, WholeEnchilada ) {
/// Test real data /// Test real data
TEST( TOAFactor, RealExperiment1 ) { TEST( TOAFactor, RealExperiment1 ) {
static const bool verbose = false; static const bool verbose = true;
// Create microphones // Create microphones
const double height = 0.5; const double height = 0.5;
@ -170,10 +174,14 @@ TEST( TOAFactor, RealExperiment1 ) {
for (size_t i = 0; i < 4; i++) { for (size_t i = 0; i < 4; i++) {
Expression<Point3> mic_(microphones[i]); // constant expression Expression<Point3> mic_(microphones[i]); // constant expression
for (size_t j = 0; j < 15; j++) for (size_t j = 0; j < 15; j++)
graph.add(makeFactor(data[j][i], eventExpressions[j], mic_)); graph.add(MakeFactor(data[j][i], eventExpressions[j], mic_));
} }
/// Print the graph // Add height priors
for (size_t j = 0; j < 15; j++)
graph.add(MakePrior((212 - 45) * cm, eventExpressions[j]));
/// Print the graph
if (verbose) if (verbose)
GTSAM_PRINT(graph); GTSAM_PRINT(graph);