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

Removed misguided Factory attempt in favor of direct expressions

release/4.3a0
dellaert 2014-12-12 07:31:33 +01:00
parent a9121fc3fc
commit 8cc813f03e
2 changed files with 64 additions and 115 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
gtsam_unstable/nonlinear/ExpressionFactor.h
View File

@ -111,88 +111,5 @@ public:
};
// ExpressionFactor
/**
* ExpressionFactor Factories
* They obviate the need for making Factor classes that are almost empty.
* They also takes a default noise model.
* TODO: Ternary version
*/
/**
* A functor that creates unary expression factors on demand
* Example usage:
* MakeUnaryFactor<double, Event> MakePrior(&Event::height, model);
* ExpressionFactor<double> factor = MakePrior(z, eventExpr);
*/
template<typename T, typename A1>
class MakeUnaryFactor {
typedef typename UnaryExpression<T, A1>::Method Method;
typedef typename UnaryExpression<T, A1>::Function Function;
Function function_;
SharedNoiseModel model_;
public:
/// Constructor with a binary function
MakeUnaryFactor(Function function, const SharedNoiseModel& model) :
function_(function), model_(model) {
}
/// Constructor with a unary method pointer
MakeUnaryFactor(Method method, const SharedNoiseModel& model) :
function_(boost::bind(method, _1, _2)), model_(model) {
}
/// Operator just needs noise model, measurement, and two expressions
ExpressionFactor<T> operator()(double measurement,
const Expression<A1>& expression1) {
// Create expression and return factor
Expression<T> expression(function_, expression1);
return ExpressionFactor<T>(model_, measurement, expression);
}
};
/**
* A functor that creates binary expression factors on demand
* Example usage:
* MakeBinaryFactor<double, Event, Point3> MakeFactor(&Event::toa, model);
* ExpressionFactor<double> factor = MakeFactor(z, eventExpr, microphoneExpr);
*/
template<typename T, typename A1, typename A2>
class MakeBinaryFactor {
typedef typename BinaryExpression<T, A1, A2>::Method Method;
typedef typename BinaryExpression<T, A1, A2>::Function Function;
Function function_;
SharedNoiseModel model_;
public:
/// Constructor with a binary function
MakeBinaryFactor(Function function, const SharedNoiseModel& model) :
function_(function), model_(model) {
}
/// Constructor with a unary method pointer
MakeBinaryFactor(Method method, const SharedNoiseModel& model) :
function_(boost::bind(method, _1, _2, _3, _4)), model_(model) {
}
/// Operator just needs noise model, measurement, and two expressions
ExpressionFactor<T> operator()(double measurement,
const Expression<A1>& expression1, const Expression<A2>& expression2) {
// Create expression and return factor
Expression<T> expression(function_, expression1, expression2);
return ExpressionFactor<T>(model_, measurement, expression);
}
};
} // \ namespace gtsam

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

@ -30,30 +30,57 @@
using namespace std;
using namespace gtsam;
// Create a noise model for the TOA error
// typedefs
typedef Eigen::Matrix<double, 1, 1> Vector1;
typedef Expression<double> Double_;
typedef Expression<Point3> Point3_;
typedef Expression<Event> Event_;
// units
static const double ms = 1e-3;
static const double cm = 1e-2;
typedef Eigen::Matrix<double, 1, 1> Vector1;
// Create a noise model for the TOA error
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms));
// And for height prior
static SharedNoiseModel heightModel(noiseModel::Isotropic::Sigma(1, 100 * cm));
static const double timeOfEvent = 25;
static const Event exampleEvent(timeOfEvent, 1, 0, 0);
static const Point3 microphoneAt0;
// A TOA factor factory
static MakeBinaryFactor<double, Event, Point3> MakeFactor(&Event::toa, model);
/**
* Factor graph that supports adding Expression Factors directly
*/
class ExpressionFactorGraph: public NonlinearFactorGraph {
// A height prior factory
static SharedNoiseModel heightModel(noiseModel::Isotropic::Sigma(1, 100*cm));
static MakeUnaryFactor<double, Event> MakePrior(&Event::height, heightModel);
public:
/// @name Adding Factors
/// @{
/**
* Add an Expression factor directly
* Which implements |h(x)-z|^2_R
*/
template<typename T>
void addExpressionFactor(const Expression<T>& h, const T& z,
const SharedNoiseModel& R) {
push_back(boost::make_shared<ExpressionFactor<T> >(R, z, h));
}
/// @}
};
//*****************************************************************************
TEST( TOAFactor, NewWay ) {
Key key = 12;
Expression<Event> eventExpression(key);
Expression<Point3> microphoneConstant(microphoneAt0); // constant expression
double z = 7;
ExpressionFactor<double> factor = MakeFactor(z, eventExpression, microphoneConstant);
Event_ eventExpression(key);
Point3_ microphoneConstant(microphoneAt0); // constant expression
double measurement = 7;
Double_ expression(&Event::toa, eventExpression, microphoneConstant);
ExpressionFactor<double> factor(model, measurement, expression);
}
//*****************************************************************************
@ -67,7 +94,7 @@ TEST( TOAFactor, WholeEnchilada ) {
microphones.push_back(Point3(0, 0, height));
microphones.push_back(Point3(403 * cm, 0, height));
microphones.push_back(Point3(403 * cm, 403 * cm, height));
microphones.push_back(Point3(0, 403 * cm, 2*height));
microphones.push_back(Point3(0, 403 * cm, 2 * height));
EXPECT_LONGS_EQUAL(4, microphones.size());
// microphones.push_back(Point3(200 * cm, 200 * cm, height));
@ -75,24 +102,25 @@ TEST( TOAFactor, WholeEnchilada ) {
const double timeOfEvent = 0;
Event groundTruthEvent(timeOfEvent, 245 * cm, 201.5 * cm, (212 - 45) * cm);
// Simulate measurements
// Simulate simulatedTOA
size_t K = microphones.size();
vector<double> measurements(K);
vector<double> simulatedTOA(K);
for (size_t i = 0; i < K; i++) {
measurements[i] = groundTruthEvent.toa(microphones[i]);
simulatedTOA[i] = groundTruthEvent.toa(microphones[i]);
if (verbose) {
cout << "mic" << i << " = " << microphones[i] << endl;
cout << "z" << i << " = " << measurements[i] / ms << endl;
cout << "z" << i << " = " << simulatedTOA[i] / ms << endl;
}
}
// Now, estimate using non-linear optimization
NonlinearFactorGraph graph;
ExpressionFactorGraph graph;
Key key = 12;
Expression<Event> eventExpression(key);
Event_ eventExpression(key);
for (size_t i = 0; i < K; i++) {
Expression<Point3> microphoneConstant(microphones[i]); // constant expression
graph.add(MakeFactor(measurements[i], eventExpression, microphoneConstant));
Point3_ microphone_i(microphones[i]); // constant expression
Double_ predictTOA(&Event::toa, eventExpression, microphone_i);
graph.addExpressionFactor(predictTOA, simulatedTOA[i], model);
}
/// Print the graph
@ -129,9 +157,9 @@ TEST( TOAFactor, RealExperiment1 ) {
static const bool verbose = true;
// Create microphones
// Create constant expressions for microphones
const double height = 0.5;
vector<Point3> microphones;
vector<Point3_> microphones;
microphones.push_back(Point3(0, 0, height));
microphones.push_back(Point3(403 * cm, 0, height));
microphones.push_back(Point3(403 * cm, 403 * cm, height));
@ -159,29 +187,33 @@ TEST( TOAFactor, RealExperiment1 ) {
// Create unknowns and initial estimate
Event nullEvent(3, 403 / 2 * cm, 403 / 2 * cm, (212 - 45) * cm);
Values initialEstimate;
vector<Expression<Event> > eventExpressions;
vector<Event_> unknownEvents;
for (size_t j = 0; j < 15; j++) {
initialEstimate.insert(j, nullEvent);
eventExpressions.push_back(Expression<Event>(j));
unknownEvents.push_back(Event_(j));
}
// Print
if (verbose)
initialEstimate.print("Initial Estimate:\n");
// Create factor graph and initial estimate
NonlinearFactorGraph graph;
// Create factor graph with TOA factors
ExpressionFactorGraph graph;
for (size_t i = 0; i < 4; i++) {
Expression<Point3> mic_(microphones[i]); // constant expression
for (size_t j = 0; j < 15; j++)
graph.add(MakeFactor(data[j][i], eventExpressions[j], mic_));
for (size_t j = 0; j < 15; j++) {
Double_ predictTOA_ij(&Event::toa, unknownEvents[j], microphones[i]);
graph.addExpressionFactor(predictTOA_ij, data[j][i], model);
}
}
// Add height priors
for (size_t j = 0; j < 15; j++)
graph.add(MakePrior((212 - 45) * cm, eventExpressions[j]));
const double heightPrior = (212 - 45) * cm;
for (size_t j = 0; j < 15; j++) {
Double_ height_j(&Event::height, unknownEvents[j]);
graph.addExpressionFactor(height_j, heightPrior, heightModel);
}
/// Print the graph
/// Print the graph
if (verbose)
GTSAM_PRINT(graph);