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

Update commment syntax and replace typedef with using

release/4.3a0
Milo Knowles 2021-06-01 16:31:20 -04:00
parent f584537412
commit 5e2af67a74
1 changed files with 15 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
gtsam_unstable/slam/MagPoseFactor.h
View File

@ -27,10 +27,10 @@ namespace gtsam {
template <class POSE> template <class POSE>
class MagPoseFactor: public NoiseModelFactor1<POSE> { class MagPoseFactor: public NoiseModelFactor1<POSE> {
private: private:
typedef MagPoseFactor<POSE> This; using This = MagPoseFactor<POSE>;
typedef NoiseModelFactor1<POSE> Base; using Base = NoiseModelFactor1<POSE>;
typedef typename POSE::Translation Point; // Could be a Vector2 or Vector3 depending on POSE. using Point = typename POSE::Translation; ///< Could be a Vector2 or Vector3 depending on POSE.
typedef typename POSE::Rotation Rot; using Rot = typename POSE::Rotation;
const Point measured_; ///< The measured magnetometer data in the body frame. const Point measured_; ///< The measured magnetometer data in the body frame.
const Point nM_; ///< Local magnetic field (mag output units) in the nav frame. const Point nM_; ///< Local magnetic field (mag output units) in the nav frame.
@ -42,22 +42,22 @@ class MagPoseFactor: public NoiseModelFactor1<POSE> {
static const int RotDim = traits<Rot>::dimension; static const int RotDim = traits<Rot>::dimension;
/// Shorthand for a smart pointer to a factor. /// Shorthand for a smart pointer to a factor.
typedef typename boost::shared_ptr<MagPoseFactor<POSE>> shared_ptr; using shared_ptr = boost::shared_ptr<MagPoseFactor<POSE>>;
/** Concept check by type. */ /// Concept check by type.
GTSAM_CONCEPT_TESTABLE_TYPE(POSE) GTSAM_CONCEPT_TESTABLE_TYPE(POSE)
GTSAM_CONCEPT_POSE_TYPE(POSE) GTSAM_CONCEPT_POSE_TYPE(POSE)
public: public:
~MagPoseFactor() override {} ~MagPoseFactor() override {}
/** Default constructor - only use for serialization. */ /// Default constructor - only use for serialization.
MagPoseFactor() {} MagPoseFactor() {}
/** /**
* Construct the factor. * Construct the factor.
* @param pose_key of the unknown pose nPb in the factor graph * @param pose_key of the unknown pose nPb in the factor graph
* @param measurement magnetometer reading, a Point2 or Point3 * @param measured magnetometer reading, a Point2 or Point3
* @param scale by which a unit vector is scaled to yield a magnetometer reading * @param scale by which a unit vector is scaled to yield a magnetometer reading
* @param direction of the local magnetic field, see e.g. http://www.ngdc.noaa.gov/geomag-web/#igrfwmm * @param direction of the local magnetic field, see e.g. http://www.ngdc.noaa.gov/geomag-web/#igrfwmm
* @param bias of the magnetometer, modeled as purely additive (after scaling) * @param bias of the magnetometer, modeled as purely additive (after scaling)
@ -83,9 +83,9 @@ class MagPoseFactor: public NoiseModelFactor1<POSE> {
NonlinearFactor::shared_ptr(new This(*this))); NonlinearFactor::shared_ptr(new This(*this)));
} }
/** Implement functions needed for Testable */ /// Implement functions needed for Testable.
/** print */ // Print out the factor.
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(s, keyFormatter); Base::print(s, keyFormatter);
gtsam::print(Vector(nM_), "local field (nM): "); gtsam::print(Vector(nM_), "local field (nM): ");
@ -93,7 +93,7 @@ class MagPoseFactor: public NoiseModelFactor1<POSE> {
gtsam::print(Vector(bias_), "magnetometer bias: "); gtsam::print(Vector(bias_), "magnetometer bias: ");
} }
/** equals */ /// Equals function.
bool equals(const NonlinearFactor& expected, double tol=1e-9) const override { bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
const This *e = dynamic_cast<const This*> (&expected); const This *e = dynamic_cast<const This*> (&expected);
return e != nullptr && Base::equals(*e, tol) && return e != nullptr && Base::equals(*e, tol) &&
@ -102,9 +102,10 @@ class MagPoseFactor: public NoiseModelFactor1<POSE> {
gtsam::equal_with_abs_tol(this->bias_, e->bias_, tol); gtsam::equal_with_abs_tol(this->bias_, e->bias_, tol);
} }
/** Implement functions needed to derive from Factor. */ /// Implement functions needed to derive from Factor.
/** Return the factor's error h(x) - z, and the optional Jacobian. Note that /**
* Return the factor's error h(x) - z, and the optional Jacobian. Note that
* the measurement error is expressed in the body frame. * the measurement error is expressed in the body frame.
*/ */
Vector evaluateError(const POSE& nPb, boost::optional<Matrix&> H = boost::none) const override { Vector evaluateError(const POSE& nPb, boost::optional<Matrix&> H = boost::none) const override {
@ -124,7 +125,7 @@ class MagPoseFactor: public NoiseModelFactor1<POSE> {
} }
private: private:
/** Serialization function */ /// Serialization function.
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) { void serialize(ARCHIVE & ar, const unsigned int /*version*/) {