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,
const KeyFormatter& keyFormatter) const {
cout << s << "AttitudeFactor on " << keyFormatter(this->key()) << "\n";
z_.print(" measured direction: ");
ref_.print(" reference direction: ");
nZ_.print(" measured direction in nav frame: ");
bRef_.print(" reference direction in body frame: ");
this->noiseModel_->print(" noise model: ");
}
//***************************************************************************
bool AttitudeFactor::equals(const NonlinearFactor& expected, double tol) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol) && this->z_.equals(e->z_, tol)
&& this->ref_.equals(e->ref_, tol);
return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
&& this->bRef_.equals(e->bRef_, tol);
}
//***************************************************************************
Vector AttitudeFactor::evaluateError(const Pose3& p,
boost::optional<Matrix&> H) const {
const Rot3& R = p.rotation();
const Rot3& nRb = p.rotation();
if (H) {
Matrix D_q_R, D_e_q;
Sphere2 q = R.rotate(z_, D_q_R);
Vector e = ref_.error(q, D_e_q);
Matrix D_nRef_R, D_e_nRef;
Sphere2 nRef = nRb.rotate(bRef_, D_nRef_R);
Vector e = nZ_.error(nRef, D_e_nRef);
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);
return e;
} else {
Sphere2 q = R * z_;
return ref_.error(q);
Sphere2 nRef = nRb * bRef_;
return nZ_.error(nRef);
}
}

16
gtsam/navigation/AttitudeFactor.h
View File

@ -37,7 +37,7 @@ private:
typedef NoiseModelFactor1<Pose3> Base;
Sphere2 z_, ref_; ///< Position measurement in
Sphere2 nZ_, bRef_; ///< Position measurement in
public:
@ -57,13 +57,13 @@ public:
/**
* @brief Constructor
* @param key of the Pose3 variable that will be constrained
* @param z measured direction in body frame
* @param ref reference direction in navigation frame
* @param nZ measured direction in navigation frame
* @param model Gaussian noise model
* @param bRef reference direction in body frame (default Z-axis)
*/
AttitudeFactor(Key key, const Sphere2& z, const Sphere2& ref,
const SharedNoiseModel& model) :
Base(model, key), z_(z), ref_(ref) {
AttitudeFactor(Key key, const Sphere2& nZ,
const SharedNoiseModel& model, const Sphere2& bRef=Sphere2(0,0,1)) :
Base(model, key), nZ_(nZ), bRef_(bRef) {
}
/// @return a deep copy of this factor
@ -96,8 +96,8 @@ private:
ar
& boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(z_);
ar & BOOST_SERIALIZATION_NVP(ref_);
ar & BOOST_SERIALIZATION_NVP(nZ_);
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.
// If pitch and roll are zero for an aerospace frame,
// that means Z is pointing down, i.e., direction of Z = (0,0,-1)
Sphere2 bZ(0, 0, 1); // body Z axis
Sphere2 nDown(0, 0, -1); // down, in ENU navigation frame
Sphere2 bZ(0, 0, 1); // reference direction is body Z axis
Sphere2 nDown(0, 0, -1); // down, in ENU navigation frame, is "measurement"
// Factor
Key key(1);
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
Pose3 T(Rot3(), Point3(-5.0, 8.0, -11.0));