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

Checked in proposed change to *any* cartesian frame

release/4.3a0
dellaert 2014-01-24 16:18:08 -05:00
parent 3476264e21
commit 8e96f6a0d4
2 changed files with 9 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
gtsam/navigation/GPSFactor.h
View File

@ -22,20 +22,8 @@
namespace gtsam { namespace gtsam {
/// Geodetic coordinates
class Geodetic {
private:
double latitude_, longitude_, altitude_;
Geodetic(double latitude, double longitude, double altitude);
public:
Point3 toNED(const Geodetic& originNED) const {
return Point3();
}
};
/** /**
* Prior on position in a local North-East-Down (NED) navigation frame * Prior on position in a local North-East-Down () navigation frame
* @addtogroup Navigation * @addtogroup Navigation
*/ */
class GPSFactor: public NoiseModelFactor1<Pose3> { class GPSFactor: public NoiseModelFactor1<Pose3> {
@ -44,7 +32,7 @@ private:
typedef NoiseModelFactor1<Pose3> Base; typedef NoiseModelFactor1<Pose3> Base;
Point3 nT_; ///< Position measurement in NED Point3 nT_; ///< Position measurement in
public: public:
@ -62,25 +50,14 @@ public:
} }
/** /**
* Constructor from a measurement already in local NED * @brief Constructor from a measurement in a Cartesian frame.
* Use GeographicLib to convert from geographic (latitude and longitude) coordinates
* @param key of the Pose3 variable that will be constrained * @param key of the Pose3 variable that will be constrained
* @param gpsInNED measurement already in NED coordinates * @param gpsIn measurement already in coordinates
* @param model Gaussian noise model * @param model Gaussian noise model
*/ */
GPSFactor(Key key, const Point3& gpsInNED, const SharedNoiseModel& model) : GPSFactor(Key key, const Point3& gpsIn, const SharedNoiseModel& model) :
Base(model, key), nT_(gpsInNED) { Base(model, key), nT_(gpsIn) {
}
/**
* Constructor that converts geodetic to local NED
* @param key of the Pose3 variable that will be constrained
* @param gps measurement
* @param originNED the origin of the NED frame
* @param model Gaussian noise model
*/
GPSFactor(Key key, const Geodetic& gps, const Geodetic& originNED,
const SharedNoiseModel& model) :
Base(model, key), nT_(gps.toNED(originNED)) {
} }
/// @return a deep copy of this factor /// @return a deep copy of this factor
@ -119,7 +96,7 @@ public:
return nT_.localCoordinates(p.translation()); return nT_.localCoordinates(p.translation());
} }
const Point3 & measurementInNED() const { const Point3 & measurementIn() const {
return nT_; return nT_;
} }

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

@ -27,10 +27,9 @@ using namespace gtsam;
TEST( GPSFactor, Constructors ) { TEST( GPSFactor, Constructors ) {
Key key(1); Key key(1);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
// TODO: convert from GPS to NED
Point3 gpsInNED; Point3 gpsInNED;
GPSFactor::Geodetic gps, originNED;
GPSFactor factor1(key, gpsInNED, model); GPSFactor factor1(key, gpsInNED, model);
GPSFactor factor2(key, gps, originNED, model);
} }
// ************************************************************************* // *************************************************************************