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

Fixed some things in GPSFactor

release/4.3a0
dellaert 2014-01-28 16:24:59 -05:00
parent 6bda38583f
commit 40f1858fd8
2 changed files with 7 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/navigation/GPSFactor.h
View File

@ -23,7 +23,7 @@
namespace gtsam {
/**
* Prior on position in a cartesian frame.
* Prior on position in a Cartesian frame.
* Possibilities include:
* ENU: East-North-Up navigation frame at some local origin
* NED: North-East-Down navigation frame at some local origin

10
gtsam/navigation/tests/testGPSFactor.cpp
View File

@ -17,15 +17,16 @@
*/
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <GeographicLib/LocalCartesian.hpp>
using namespace std;
using namespace GeographicLib;
using namespace gtsam;
using namespace GeographicLib;
// *************************************************************************
TEST( GPSFactor, Constructors ) {
@ -50,8 +51,9 @@ TEST( GPSFactor, Constructors ) {
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
GPSFactor factor(key, Point3(E, N, U), model);
// Create a linearization point at the zero-error point
Pose3 T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
// Create a linearization point at zero error
Pose3 T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U));
CHECK(assert_equal(zero(3),factor.evaluateError(T),1e-5));
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Pose3>(