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 { namespace gtsam {
/** /**
* Prior on position in a cartesian frame. * Prior on position in a Cartesian frame.
* Possibilities include: * Possibilities include:
* ENU: East-North-Up navigation frame at some local origin * ENU: East-North-Up navigation frame at some local origin
* NED: North-East-Down 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/navigation/GPSFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <GeographicLib/LocalCartesian.hpp> #include <GeographicLib/LocalCartesian.hpp>
using namespace std; using namespace std;
using namespace GeographicLib;
using namespace gtsam; using namespace gtsam;
using namespace GeographicLib;
// ************************************************************************* // *************************************************************************
TEST( GPSFactor, Constructors ) { TEST( GPSFactor, Constructors ) {
@ -50,8 +51,9 @@ TEST( GPSFactor, Constructors ) {
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
GPSFactor factor(key, Point3(E, N, U), model); GPSFactor factor(key, Point3(E, N, U), model);
// Create a linearization point at the zero-error point // Create a linearization point at zero error
Pose3 T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); 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 // Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Pose3>( Matrix expectedH = numericalDerivative11<Pose3>(