Skip to content

Commit

Permalink
Merge pull request #1856 from borglab/feature/attitude
Browse files Browse the repository at this point in the history 
Navigation docs
  • Loading branch information
@dellaert
dellaert authored Oct 30, 2024
2 parents d48b1fc + 38ffcc3 commit ecc7366
Show file tree
Hide file tree
Showing 7 changed files with 317 additions and 144 deletions.
30 changes: 15 additions & 15 deletions gtsam/navigation/AttitudeFactor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,16 +26,16 @@ namespace gtsam {
Vector AttitudeFactor::attitudeError(const Rot3& nRb,
OptionalJacobian<2, 3> H) const {
if (H) {
Matrix23 D_nRef_R;
Matrix22 D_e_nRef;
Unit3 nRef = nRb.rotate(bRef_, D_nRef_R);
Vector e = nZ_.error(nRef, D_e_nRef);
Matrix23 D_nRotated_R;
Matrix22 D_e_nRotated;
Unit3 nRotated = nRb.rotate(bMeasured_, D_nRotated_R);
Vector e = nRef_.error(nRotated, D_e_nRotated);

(*H) = D_e_nRef * D_nRef_R;
(*H) = D_e_nRotated * D_nRotated_R;
return e;
} else {
Unit3 nRef = nRb * bRef_;
return nZ_.error(nRef);
Unit3 nRotated = nRb * bMeasured_;
return nRef_.error(nRotated);
}
}

Expand All @@ -44,34 +44,34 @@ void Rot3AttitudeFactor::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << (s.empty() ? "" : s + " ") << "Rot3AttitudeFactor on "
<< keyFormatter(this->key()) << "\n";
nZ_.print(" measured direction in nav frame: ");
bRef_.print(" reference direction in body frame: ");
nRef_.print(" reference direction in nav frame: ");
bMeasured_.print(" measured 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 != nullptr && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
&& this->bRef_.equals(e->bRef_, tol);
return e != nullptr && Base::equals(*e, tol) && this->nRef_.equals(e->nRef_, tol)
&& this->bMeasured_.equals(e->bMeasured_, tol);
}

//***************************************************************************
void Pose3AttitudeFactor::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << s << "Pose3AttitudeFactor on " << keyFormatter(this->key()) << "\n";
nZ_.print(" measured direction in nav frame: ");
bRef_.print(" reference direction in body frame: ");
nRef_.print(" reference direction in nav frame: ");
bMeasured_.print(" measured direction in body frame: ");
this->noiseModel_->print(" noise model: ");
}

//***************************************************************************
bool Pose3AttitudeFactor::equals(const NonlinearFactor& expected,
double tol) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != nullptr && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
&& this->bRef_.equals(e->bRef_, tol);
return e != nullptr && Base::equals(*e, tol) && this->nRef_.equals(e->nRef_, tol)
&& this->bMeasured_.equals(e->bMeasured_, tol);
}

//***************************************************************************
Expand Down
172 changes: 88 additions & 84 deletions gtsam/navigation/AttitudeFactor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,59 +17,69 @@
**/
#pragma once

#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>

namespace gtsam {

/**
* 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
* @class AttitudeFactor
*
* @brief Base class for an attitude factor that constrains the rotation between
* body and navigation frames.
*
* This factor enforces that when the measured direction in the body frame
* (e.g., from an IMU accelerometer) is rotated into the navigation frame using the
* rotation variable, it should align with a known reference direction in the
* navigation frame (e.g., gravity vector).
*
* Mathematically, the error is zero when:
* nRb * bMeasured == nRef
*
* This is useful for incorporating absolute orientation measurements into the
* factor graph.
*
* @ingroup navigation
*/
class AttitudeFactor {
protected:
Unit3 nRef_, bMeasured_; ///< Position measurement in

protected:

Unit3 nZ_, bRef_; ///< Position measurement in

public:

public:
/** default constructor - only use for serialization */
AttitudeFactor() {
}
AttitudeFactor() {}

/**
* @brief Constructor
* @param nZ measured direction in navigation frame
* @param bRef reference direction in body frame (default Z-axis in NED frame, i.e., [0; 0; 1])
* @param nRef Reference direction in the navigation frame (e.g., gravity).
* @param bMeasured Measured direction in the body frame (e.g., from IMU
* accelerometer). Default is Unit3(0, 0, 1).
*/
AttitudeFactor(const Unit3& nZ, const Unit3& bRef = Unit3(0, 0, 1)) :
nZ_(nZ), bRef_(bRef) {
}
AttitudeFactor(const Unit3& nRef, const Unit3& bMeasured = Unit3(0, 0, 1))
: nRef_(nRef), bMeasured_(bMeasured) {}

/** vector of errors */
Vector attitudeError(const Rot3& p,
OptionalJacobian<2,3> H = {}) const;
Vector attitudeError(const Rot3& p, OptionalJacobian<2, 3> H = {}) const;

const Unit3& nZ() const {
return nZ_;
}
const Unit3& bRef() const {
return bRef_;
}
const Unit3& nRef() const { return nRef_; }
const Unit3& bMeasured() const { return bMeasured_; }

#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
[[deprecated("Use nRef() instead")]]
const Unit3& nZ() const { return nRef_; }

[[deprecated("Use bMeasured() instead")]]
const Unit3& bRef() const { return bMeasured_; }
#endif

#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("nZ_", nZ_);
ar & boost::serialization::make_nvp("bRef_", bRef_);
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp("nRef_", nRef_);
ar& boost::serialization::make_nvp("bMeasured_", bMeasured_);
}
#endif
};
Expand All @@ -78,12 +88,11 @@ class AttitudeFactor {
* Version of AttitudeFactor for Rot3
* @ingroup navigation
*/
class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactorN<Rot3>, public AttitudeFactor {

class GTSAM_EXPORT Rot3AttitudeFactor : public NoiseModelFactorN<Rot3>,
public AttitudeFactor {
typedef NoiseModelFactorN<Rot3> Base;

public:

public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;

Expand All @@ -94,23 +103,20 @@ class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactorN<Rot3>, public At
typedef Rot3AttitudeFactor This;

/** default constructor - only use for serialization */
Rot3AttitudeFactor() {
}
Rot3AttitudeFactor() {}

~Rot3AttitudeFactor() override {
}
~Rot3AttitudeFactor() override {}

/**
* @brief Constructor
* @param key of the Rot3 variable that will be constrained
* @param nZ measured direction in navigation frame
* @param nRef reference direction in navigation frame
* @param model Gaussian noise model
* @param bRef reference direction in body frame (default Z-axis)
* @param bMeasured measured direction in body frame (default Z-axis)
*/
Rot3AttitudeFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model,
const Unit3& bRef = Unit3(0, 0, 1)) :
Base(model, key), AttitudeFactor(nZ, bRef) {
}
Rot3AttitudeFactor(Key key, const Unit3& nRef, const SharedNoiseModel& model,
const Unit3& bMeasured = Unit3(0, 0, 1))
: Base(model, key), AttitudeFactor(nRef, bMeasured) {}

/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
Expand All @@ -123,46 +129,46 @@ class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactorN<Rot3>, public At
DefaultKeyFormatter) const override;

/** equals */
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
bool equals(const NonlinearFactor& expected,
double tol = 1e-9) const override;

/** vector of errors */
Vector evaluateError(const Rot3& nRb, OptionalMatrixType H) const override {
return attitudeError(nRb, H);
}

private:

private:
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & boost::serialization::make_nvp("AttitudeFactor",
ar& boost::serialization::make_nvp(
"NoiseModelFactor1", boost::serialization::base_object<Base>(*this));
ar& boost::serialization::make_nvp(
"AttitudeFactor",
boost::serialization::base_object<AttitudeFactor>(*this));
}
#endif

public:
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};

/// traits
template<> struct traits<Rot3AttitudeFactor> : public Testable<Rot3AttitudeFactor> {};
template <>
struct traits<Rot3AttitudeFactor> : public Testable<Rot3AttitudeFactor> {};

/**
* Version of AttitudeFactor for Pose3
* @ingroup navigation
*/
class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactorN<Pose3>,
public AttitudeFactor {

class GTSAM_EXPORT Pose3AttitudeFactor : public NoiseModelFactorN<Pose3>,
public AttitudeFactor {
typedef NoiseModelFactorN<Pose3> Base;

public:

public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;

Expand All @@ -173,23 +179,20 @@ class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactorN<Pose3>,
typedef Pose3AttitudeFactor This;

/** default constructor - only use for serialization */
Pose3AttitudeFactor() {
}
Pose3AttitudeFactor() {}

~Pose3AttitudeFactor() override {
}
~Pose3AttitudeFactor() override {}

/**
* @brief Constructor
* @param key of the Pose3 variable that will be constrained
* @param nZ measured direction in navigation frame
* @param nRef reference direction in navigation frame
* @param model Gaussian noise model
* @param bRef reference direction in body frame (default Z-axis)
* @param bMeasured measured direction in body frame (default Z-axis)
*/
Pose3AttitudeFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model,
const Unit3& bRef = Unit3(0, 0, 1)) :
Base(model, key), AttitudeFactor(nZ, bRef) {
}
Pose3AttitudeFactor(Key key, const Unit3& nRef, const SharedNoiseModel& model,
const Unit3& bMeasured = Unit3(0, 0, 1))
: Base(model, key), AttitudeFactor(nRef, bMeasured) {}

/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
Expand All @@ -202,40 +205,41 @@ class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactorN<Pose3>,
DefaultKeyFormatter) const override;

/** equals */
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
bool equals(const NonlinearFactor& expected,
double tol = 1e-9) const override;

/** vector of errors */
Vector evaluateError(const Pose3& nTb, OptionalMatrixType H) const override {
Vector e = attitudeError(nTb.rotation(), H);
if (H) {
Matrix H23 = *H;
*H = Matrix::Zero(2,6);
H->block<2,3>(0,0) = H23;
*H = Matrix::Zero(2, 6);
H->block<2, 3>(0, 0) = H23;
}
return e;
}

private:

private:
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & boost::serialization::make_nvp("AttitudeFactor",
ar& boost::serialization::make_nvp(
"NoiseModelFactor1", boost::serialization::base_object<Base>(*this));
ar& boost::serialization::make_nvp(
"AttitudeFactor",
boost::serialization::base_object<AttitudeFactor>(*this));
}
#endif

public:
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};

/// traits
template<> struct traits<Pose3AttitudeFactor> : public Testable<Pose3AttitudeFactor> {};

} /// namespace gtsam
template <>
struct traits<Pose3AttitudeFactor> : public Testable<Pose3AttitudeFactor> {};

} // namespace gtsam
Loading

0 comments on commit ecc7366

Please sign in to comment.