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

Changed API slightly and made very clear what is measurement (typically in nav frame) and what is reference direction (typically in body frame)

release/4.3a0
dellaert 2014-01-29 13:34:52 -05:00
parent 19ed8b93b4
commit 8fbab3bde8
3 changed files with 24 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
gtsam/navigation/AttitudeFactor.cpp
View File

@ -26,33 +26,33 @@ namespace gtsam {
void AttitudeFactor::print(const string& s, void AttitudeFactor::print(const string& s,
const KeyFormatter& keyFormatter) const { const KeyFormatter& keyFormatter) const {
cout << s << "AttitudeFactor on " << keyFormatter(this->key()) << "\n"; cout << s << "AttitudeFactor on " << keyFormatter(this->key()) << "\n";
z_.print(" measured direction: "); nZ_.print(" measured direction in nav frame: ");
ref_.print(" reference direction: "); bRef_.print(" reference direction in body frame: ");
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");
} }
//*************************************************************************** //***************************************************************************
bool AttitudeFactor::equals(const NonlinearFactor& expected, double tol) const { bool AttitudeFactor::equals(const NonlinearFactor& expected, double tol) const {
const This* e = dynamic_cast<const This*>(&expected); const This* e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol) && this->z_.equals(e->z_, tol) return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
&& this->ref_.equals(e->ref_, tol); && this->bRef_.equals(e->bRef_, tol);
} }
//*************************************************************************** //***************************************************************************
Vector AttitudeFactor::evaluateError(const Pose3& p, Vector AttitudeFactor::evaluateError(const Pose3& p,
boost::optional<Matrix&> H) const { boost::optional<Matrix&> H) const {
const Rot3& R = p.rotation(); const Rot3& nRb = p.rotation();
if (H) { if (H) {
Matrix D_q_R, D_e_q; Matrix D_nRef_R, D_e_nRef;
Sphere2 q = R.rotate(z_, D_q_R); Sphere2 nRef = nRb.rotate(bRef_, D_nRef_R);
Vector e = ref_.error(q, D_e_q); Vector e = nZ_.error(nRef, D_e_nRef);
H->resize(2, 6); H->resize(2, 6);
H->block < 2, 3 > (0, 0) = D_e_q * D_q_R; H->block < 2, 3 > (0, 0) = D_e_nRef * D_nRef_R;
H->block < 2, 3 > (0, 3) << Matrix::Zero(2, 3); H->block < 2, 3 > (0, 3) << Matrix::Zero(2, 3);
return e; return e;
} else { } else {
Sphere2 q = R * z_; Sphere2 nRef = nRb * bRef_;
return ref_.error(q); return nZ_.error(nRef);
} }
} }

16
gtsam/navigation/AttitudeFactor.h
View File

@ -37,7 +37,7 @@ private:
typedef NoiseModelFactor1<Pose3> Base; typedef NoiseModelFactor1<Pose3> Base;
Sphere2 z_, ref_; ///< Position measurement in Sphere2 nZ_, bRef_; ///< Position measurement in
public: public:
@ -57,13 +57,13 @@ public:
/** /**
* @brief Constructor * @brief Constructor
* @param key of the Pose3 variable that will be constrained * @param key of the Pose3 variable that will be constrained
* @param z measured direction in body frame * @param nZ measured direction in navigation frame
* @param ref reference direction in navigation frame
* @param model Gaussian noise model * @param model Gaussian noise model
* @param bRef reference direction in body frame (default Z-axis)
*/ */
AttitudeFactor(Key key, const Sphere2& z, const Sphere2& ref, AttitudeFactor(Key key, const Sphere2& nZ,
const SharedNoiseModel& model) : const SharedNoiseModel& model, const Sphere2& bRef=Sphere2(0,0,1)) :
Base(model, key), z_(z), ref_(ref) { Base(model, key), nZ_(nZ), bRef_(bRef) {
} }
/// @return a deep copy of this factor /// @return a deep copy of this factor
@ -96,8 +96,8 @@ private:
ar ar
& boost::serialization::make_nvp("NoiseModelFactor1", & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(z_); ar & BOOST_SERIALIZATION_NVP(nZ_);
ar & BOOST_SERIALIZATION_NVP(ref_); ar & BOOST_SERIALIZATION_NVP(bRef_);
} }
}; };

8
gtsam/navigation/tests/testAttitudeFactor.cpp
View File

@ -30,13 +30,15 @@ TEST( AttitudeFactor, Constructor ) {
// Example: pitch and roll of aircraft in an ENU Cartesian frame. // Example: pitch and roll of aircraft in an ENU Cartesian frame.
// If pitch and roll are zero for an aerospace frame, // If pitch and roll are zero for an aerospace frame,
// that means Z is pointing down, i.e., direction of Z = (0,0,-1) // that means Z is pointing down, i.e., direction of Z = (0,0,-1)
Sphere2 bZ(0, 0, 1); // body Z axis Sphere2 bZ(0, 0, 1); // reference direction is body Z axis
Sphere2 nDown(0, 0, -1); // down, in ENU navigation frame Sphere2 nDown(0, 0, -1); // down, in ENU navigation frame, is "measurement"
// Factor // Factor
Key key(1); Key key(1);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
AttitudeFactor factor(key, bZ, nDown, model); AttitudeFactor factor0(key, nDown, model);
AttitudeFactor factor(key, nDown, model, bZ);
EXPECT(assert_equal(factor0,factor,1e-5));
// Create a linearization point at the zero-error point // Create a linearization point at the zero-error point
Pose3 T(Rot3(), Point3(-5.0, 8.0, -11.0)); Pose3 T(Rot3(), Point3(-5.0, 8.0, -11.0));