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

Changed to Point3, which makes much more sense

release/4.3a0
dellaert 2014-02-02 22:43:00 -05:00
parent a24d4ad3d1
commit 1d5da1c35e
3 changed files with 13 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam/navigation/GPSFactor.cpp
View File

@ -48,26 +48,26 @@ Vector GPSFactor::evaluateError(const Pose3& p,
} }
//*************************************************************************** //***************************************************************************
pair<Pose3, Vector3> GPSFactor::EstimateState(double t1, const Vector3& NED1, pair<Pose3, Vector3> GPSFactor::EstimateState(double t1, const Point3& NED1,
double t2, const Vector3& NED2, double timestamp) { double t2, const Point3& NED2, double timestamp) {
// Estimate initial velocity as difference in NED frame // Estimate initial velocity as difference in NED frame
double dt = t2 - t1; double dt = t2 - t1;
Vector3 nV = (NED2 - NED1) / dt; Point3 nV = (NED2 - NED1) / dt;
// Estimate initial position as linear interpolation // Estimate initial position as linear interpolation
Vector3 nT = NED1 + nV * (timestamp - t1); Point3 nT = NED1 + nV * (timestamp - t1);
// Estimate Rotation // Estimate Rotation
double yaw = atan2(nV[1], nV[0]); double yaw = atan2(nV.y(), nV.x());
Rot3 nRy = Rot3::yaw(yaw); // yaw frame Rot3 nRy = Rot3::yaw(yaw); // yaw frame
Vector3 yV = nRy.transpose() * nV; // velocity in yaw frame Point3 yV = nRy.inverse() * nV; // velocity in yaw frame
double pitch = -atan2(yV[2], yV[0]), roll = 0; double pitch = -atan2(yV.z(), yV.x()), roll = 0;
Rot3 nRb = Rot3::ypr(yaw, pitch, roll); Rot3 nRb = Rot3::ypr(yaw, pitch, roll);
// Construct initial pose // Construct initial pose
Pose3 nTb(nRb, Point3(nT[0], nT[1], nT[2])); // nTb Pose3 nTb(nRb, nT); // nTb
return make_pair(nTb, nV); return make_pair(nTb, nV.vector());
} }
//*************************************************************************** //***************************************************************************

4
gtsam/navigation/GPSFactor.h
View File

@ -95,8 +95,8 @@ public:
* readings (in local NED Cartesian frame) bracketing t * readings (in local NED Cartesian frame) bracketing t
* Assumes roll is zero, calculates yaw and pitch from NED1->NED2 vector. * Assumes roll is zero, calculates yaw and pitch from NED1->NED2 vector.
*/ */
static std::pair<Pose3, Vector3> EstimateState(double t1, const Vector3& NED1, static std::pair<Pose3, Vector3> EstimateState(double t1, const Point3& NED1,
double t2, const Vector3& NED2, double timestamp); double t2, const Point3& NED2, double timestamp);
private: private:

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

@ -72,7 +72,7 @@ TEST(GPSData, init) {
// GPS Reading 1 will be ENU origin // GPS Reading 1 will be ENU origin
double t1 = 84831; double t1 = 84831;
Vector3 NED1(0, 0, 0); Point3 NED1(0, 0, 0);
LocalCartesian enu(35.4393283333333, -119.062986666667, 275.54, LocalCartesian enu(35.4393283333333, -119.062986666667, 275.54,
Geocentric::WGS84); Geocentric::WGS84);
@ -80,7 +80,7 @@ TEST(GPSData, init) {
double t2 = 84831.5; double t2 = 84831.5;
double E, N, U; double E, N, U;
enu.Forward(35.4394633333333, -119.063146666667, 276.52, E, N, U); enu.Forward(35.4394633333333, -119.063146666667, 276.52, E, N, U);
Vector3 NED2(N, E, -U); Point3 NED2(N, E, -U);
// Estimate initial state // Estimate initial state
Pose3 T; Pose3 T;