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

Put in prototype derivatives, needs unit test

release/4.3a0
dellaert 2014-12-10 16:06:34 +01:00
parent cca1a54544
commit 0ceb09262c
1 changed files with 12 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -82,12 +82,20 @@ public:
} }
/// Time of arrival to given microphone /// Time of arrival to given microphone
double toa(const Point3& microphone, OptionalJacobian<1, 4> H1 = boost::none, double toa(const Point3& microphone, //
OptionalJacobian<1, 4> H1 = boost::none, //
OptionalJacobian<1, 3> H2 = boost::none) const { OptionalJacobian<1, 3> H2 = boost::none) const {
Matrix13 D1, D2;
double distance = location_.distance(microphone, D1, D2);
if (H1) { if (H1) {
// derivative of toa with respect to event
*H1 << 1, D1 / Speed;
} }
return time_ + location_.distance(microphone) / Speed; if (H2) {
// derivative of toa with respect to microphone location
*H2 << D2 / Speed;
}
return time_ + distance / Speed;
} }
}; };