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

Created common base class

release/4.3a0
dellaert 2014-02-01 19:05:23 -05:00
parent d0ee9c662d
commit 7169c127fa
3 changed files with 82 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
gtsam/navigation/AttitudeFactor.cpp
View File

@ -23,23 +23,7 @@ using namespace std;
namespace gtsam {
//***************************************************************************
void AttitudeFactor::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << s << "AttitudeFactor on " << keyFormatter(this->key()) << "\n";
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->nZ_.equals(e->nZ_, tol)
&& this->bRef_.equals(e->bRef_, tol);
}
//***************************************************************************
Vector AttitudeFactor::evaluateError(const Rot3& nRb,
Vector AttitudeFactor::attitudeError(const Rot3& nRb,
boost::optional<Matrix&> H) const {
if (H) {
Matrix D_nRef_R, D_e_nRef;
@ -54,6 +38,23 @@ Vector AttitudeFactor::evaluateError(const Rot3& nRb,
}
}
//***************************************************************************
void Rot3AttitudeFactor::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << s << "Rot3AttitudeFactor on " << keyFormatter(this->key()) << "\n";
nZ_.print(" measured direction in nav frame: ");
bRef_.print(" reference direction in body frame: ");
this->noiseModel_->print(" noise model: ");
}
//***************************************************************************
bool Rot3AttitudeFactor::equals(const NonlinearFactor& expected,
double tol) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
&& this->bRef_.equals(e->bRef_, tol);
}
//***************************************************************************
void Pose3AttitudeFactor::print(const string& s,
const KeyFormatter& keyFormatter) const {
@ -71,24 +72,6 @@ bool Pose3AttitudeFactor::equals(const NonlinearFactor& expected,
&& this->bRef_.equals(e->bRef_, tol);
}
//***************************************************************************
Vector Pose3AttitudeFactor::evaluateError(const Pose3& p,
boost::optional<Matrix&> H) const {
const Rot3& nRb = p.rotation();
if (H) {
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_nRef * D_nRef_R;
H->block < 2, 3 > (0, 3) << Matrix::Zero(2, 3);
return e;
} else {
Sphere2 nRef = nRb * bRef_;
return nZ_.error(nRef);
}
}
//***************************************************************************
}/// namespace gtsam

93
gtsam/navigation/AttitudeFactor.h
View File

@ -24,34 +24,60 @@
namespace gtsam {
/**
* Prior on the attitude of a Rot3.
* Base class for prior on attitude
* Example:
* - measurement is direction of gravity in body frame bF
* - reference is direction of gravity in navigation frame nG
* This factor will give zero error if nRb * bF == nG
* @addtogroup Navigation
*/
class AttitudeFactor: public NoiseModelFactor1<Rot3> {
class AttitudeFactor {
private:
protected:
typedef NoiseModelFactor1<Rot3> Base;
Sphere2 nZ_, bRef_; ///< Position measurement in
const Sphere2 nZ_, bRef_; ///< Position measurement in
public:
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<AttitudeFactor> shared_ptr;
/// Typedef to this class
typedef AttitudeFactor This;
/** default constructor - only use for serialization */
AttitudeFactor() {
}
virtual ~AttitudeFactor() {
/**
* @brief Constructor
* @param nZ measured direction in navigation frame
* @param bRef reference direction in body frame (default Z-axis)
*/
AttitudeFactor(const Sphere2& nZ, const Sphere2& bRef = Sphere2(0, 0, 1)) :
nZ_(nZ), bRef_(bRef) {
}
/** vector of errors */
Vector attitudeError(const Rot3& p,
boost::optional<Matrix&> H = boost::none) const;
};
/**
* Version of AttitudeFactor for Rot3
* @addtogroup Navigation
*/
class Rot3AttitudeFactor: public NoiseModelFactor1<Rot3>, public AttitudeFactor {
typedef NoiseModelFactor1<Rot3> Base;
public:
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<Rot3AttitudeFactor> shared_ptr;
/// Typedef to this class
typedef Rot3AttitudeFactor This;
/** default constructor - only use for serialization */
Rot3AttitudeFactor() {
}
virtual ~Rot3AttitudeFactor() {
}
/**
@ -61,9 +87,9 @@ public:
* @param model Gaussian noise model
* @param bRef reference direction in body frame (default Z-axis)
*/
AttitudeFactor(Key key, const Sphere2& nZ, const SharedNoiseModel& model,
Rot3AttitudeFactor(Key key, const Sphere2& nZ, const SharedNoiseModel& model,
const Sphere2& bRef = Sphere2(0, 0, 1)) :
Base(model, key), nZ_(nZ), bRef_(bRef) {
Base(model, key), AttitudeFactor(nZ, bRef) {
}
/// @return a deep copy of this factor
@ -72,8 +98,6 @@ public:
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/** implement functions needed for Testable */
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const;
@ -81,11 +105,11 @@ public:
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
/** implement functions needed to derive from Factor */
/** vector of errors */
Vector evaluateError(const Rot3& p,
boost::optional<Matrix&> H = boost::none) const;
virtual Vector evaluateError(const Rot3& nRb, //
boost::optional<Matrix&> H = boost::none) const {
return attitudeError(nRb, H);
}
private:
@ -102,17 +126,14 @@ private:
};
/**
* Version of AttitudeFactor for Pose3 (this should be easier)
* Version of AttitudeFactor for Pose3
* @addtogroup Navigation
*/
class Pose3AttitudeFactor: public NoiseModelFactor1<Pose3> {
private:
class Pose3AttitudeFactor: public NoiseModelFactor1<Pose3>,
public AttitudeFactor {
typedef NoiseModelFactor1<Pose3> Base;
Sphere2 nZ_, bRef_; ///< Position measurement in
public:
/// shorthand for a smart pointer to a factor
@ -137,7 +158,7 @@ public:
*/
Pose3AttitudeFactor(Key key, const Sphere2& nZ, const SharedNoiseModel& model,
const Sphere2& bRef = Sphere2(0, 0, 1)) :
Base(model, key), nZ_(nZ), bRef_(bRef) {
Base(model, key), AttitudeFactor(nZ, bRef) {
}
/// @return a deep copy of this factor
@ -146,8 +167,6 @@ public:
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/** implement functions needed for Testable */
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const;
@ -155,11 +174,17 @@ public:
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
/** implement functions needed to derive from Factor */
/** vector of errors */
Vector evaluateError(const Pose3& p,
boost::optional<Matrix&> H = boost::none) const;
virtual Vector evaluateError(const Pose3& nTb, //
boost::optional<Matrix&> H = boost::none) const {
Vector e = attitudeError(nTb.rotation(), H);
if (H) {
Matrix H23 = *H;
*H = Matrix::Zero(2, 6);
H->block<2,3>(0,0) = H23;
}
return e;
}
private:

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

@ -11,7 +11,7 @@
/**
* @file testAttitudeFactor.cpp
* @brief Unit test for AttitudeFactor
* @brief Unit test for Rot3AttitudeFactor
* @author Frank Dellaert
* @date January 22, 2014
*/
@ -25,7 +25,7 @@ using namespace std;
using namespace gtsam;
// *************************************************************************
TEST( AttitudeFactor, Constructor ) {
TEST( Rot3AttitudeFactor, Constructor ) {
// Example: pitch and roll of aircraft in an ENU Cartesian frame.
// If pitch and roll are zero for an aerospace frame,
@ -36,8 +36,8 @@ TEST( AttitudeFactor, Constructor ) {
// Factor
Key key(1);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
AttitudeFactor factor0(key, nDown, model);
AttitudeFactor factor(key, nDown, model, bZ);
Rot3AttitudeFactor factor0(key, nDown, model);
Rot3AttitudeFactor factor(key, nDown, model, bZ);
EXPECT(assert_equal(factor0,factor,1e-5));
// Create a linearization point at the zero-error point
@ -46,7 +46,7 @@ TEST( AttitudeFactor, Constructor ) {
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Rot3>(
boost::bind(&AttitudeFactor::evaluateError, &factor, _1, boost::none),
boost::bind(&Rot3AttitudeFactor::evaluateError, &factor, _1, boost::none),
nRb);
// Use the factor to calculate the derivative