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

TOAFactor compiles and can be put into a factor graph

release/4.3a0
dellaert 2014-12-10 15:43:31 +01:00
parent ba1a2685bc
commit d54c70202a
1 changed files with 45 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -58,14 +58,6 @@ const double Event::Speed = 330;
// Define GTSAM traits // Define GTSAM traits
namespace traits { namespace traits {
//template<>
//struct GTSAM_EXPORT is_group<Point2> : public boost::true_type{
//};
//
//template<>
//struct GTSAM_EXPORT is_manifold<Point2> : public boost::true_type{
//};
template<> template<>
struct GTSAM_EXPORT dimension<Event> : public boost::integral_constant<int, 4> { struct GTSAM_EXPORT dimension<Event> : public boost::integral_constant<int, 4> {
}; };
@ -140,37 +132,54 @@ TEST( TOA, Expression ) {
Point3 microphone; Point3 microphone;
Expression<Point3> knownMicrophone_(microphone); // constant expression Expression<Point3> knownMicrophone_(microphone); // constant expression
Expression<double> expression(&Event::toa, event_, knownMicrophone_); Expression<double> expression(&Event::toa, event_, knownMicrophone_);
// double timeOfEvent = 25;
// Event event12(timeOfEvent, 1, 0, 0);
// Values values;
// values.insert(key,event12);
// double expectedTOA = timeOfEvent + 1 / Event::Speed;
// EXPECT_DOUBLES_EQUAL(expectedTOA, expression.value(values), 1e-9);
} }
//***************************************************************************** //*****************************************************************************
//TEST( TOAFactor, WholeEnchilada ) { TEST( TOAFactor, Constract ) {
// Key key = 12;
// // Create microphones Expression<Event> event_(key);
// vector<Point3> microphones; Point3 microphone;
// microphones.push_back(Point3(0, 0, 0)); Expression<Point3> knownMicrophone_(microphone); // constant expression
// microphones.push_back(Point3(403 * cm, 0, 0)); double measurement = 7;
// microphones.push_back(Point3(403 * cm, 403 * cm, 0)); TOAFactor factor(event_, knownMicrophone_, measurement, model);
// microphones.push_back(Point3(0, 403 * cm, 0)); }
// EXPECT_LONGS_EQUAL(4, microphones.size());
// //*****************************************************************************
// // Create a ground truth point TEST( TOAFactor, WholeEnchilada ) {
// const double timeOfEvent = 0;
// Event event(timeOfEvent, 201.5 * cm, 201.5 * cm, (212 - 45) * cm); // Create microphones
// vector<Point3> microphones;
// // Simulate measurements microphones.push_back(Point3(0, 0, 0));
// vector<double> measurements(4); microphones.push_back(Point3(403 * cm, 0, 0));
// for (size_t i = 0; i < 4; i++) microphones.push_back(Point3(403 * cm, 403 * cm, 0));
// measurements[i] = event.toa(microphones[i]); microphones.push_back(Point3(0, 403 * cm, 0));
// EXPECT_LONGS_EQUAL(4, microphones.size());
// // Now, estimate using non-linear optimization
// NonlinearFactorGraph graph; // Create a ground truth point
// Key key = 12; const double timeOfEvent = 0;
// Expression<Event> event_(key); Event event(timeOfEvent, 201.5 * cm, 201.5 * cm, (212 - 45) * cm);
// for (size_t i = 0; i < 4; i++) {
// Expression<Point3> knownMicrophone_(microphones[i]); // constant expression // Simulate measurements
// graph.add(TOAFactor(event_, knownMicrophone_, measurements[i], model)); vector<double> measurements(4);
// } for (size_t i = 0; i < 4; i++)
//} measurements[i] = event.toa(microphones[i]);
// Now, estimate using non-linear optimization
NonlinearFactorGraph graph;
Key key = 12;
Expression<Event> event_(key);
for (size_t i = 0; i < 4; i++) {
Expression<Point3> knownMicrophone_(microphones[i]); // constant expression
graph.add(TOAFactor(event_, knownMicrophone_, measurements[i], model));
}
}
//***************************************************************************** //*****************************************************************************
int main() { int main() {