Skip to content

More navigation docs #2084

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
dellaert merged 12 commits into from
Apr 7, 2025
Merged

More navigation docs #2084

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
b29ca85
Mermaid diagram
dellaert Apr 7, 2025
12d74ad
Split AHRS and IMU overviews
dellaert Apr 7, 2025
f991a42
Split off params diagram
dellaert Apr 7, 2025
e84b1c4
Almost perfect Gemini 2.5 notebooks
dellaert Apr 7, 2025
13a74a4
AttitudeFactor
dellaert Apr 7, 2025
8757090
BarometricFactor
dellaert Apr 7, 2025
12104b7
ConstantVelocityFactor + wrapping
dellaert Apr 7, 2025
ae87ca6
GPSFactor + wrapping
dellaert Apr 7, 2025
f9c981d
MagFactor + wrapping
dellaert Apr 7, 2025
f4485db
MagPoseFactor + wrapping
dellaert Apr 7, 2025
a68d3ff
Scenario and ScenarioRunner
dellaert Apr 7, 2025
5eaba68
Add files and links
dellaert Apr 7, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion gtsam/navigation/ConstantVelocityFactor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ class ConstantVelocityFactor : public NoiseModelFactorN<NavState, NavState> {
~ConstantVelocityFactor() override {}

/**
* @brief Caclulate error: (x2 - x1.update(dt)))
* @brief Calculate error: (x2 - x1.update(dt)))
* where X1 and X1 are NavStates and dt is
* the time difference in seconds between the states.
* @param x1 NavState for key a
Expand Down
32 changes: 16 additions & 16 deletions gtsam/navigation/GPSFactor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,9 +37,9 @@ bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const {
}

//***************************************************************************
Vector GPSFactor::evaluateError(const Pose3& p,
Vector GPSFactor::evaluateError(const Pose3& nTb,
OptionalMatrixType H) const {
return p.translation(H) -nT_;
return nTb.translation(H) -nT_;
}

//***************************************************************************
Expand Down Expand Up @@ -82,17 +82,17 @@ bool GPSFactorArm::equals(const NonlinearFactor& expected, double tol) const {
}

//***************************************************************************
Vector GPSFactorArm::evaluateError(const Pose3& p,
Vector GPSFactorArm::evaluateError(const Pose3& nTb,
OptionalMatrixType H) const {
const Matrix3 nRb = p.rotation().matrix();
const Matrix3 nRb = nTb.rotation().matrix();
if (H) {
H->resize(3, 6);

H->block<3, 3>(0, 0) = -nRb * skewSymmetric(bL_);
H->block<3, 3>(0, 3) = nRb;
}

return p.translation() + nRb * bL_ - nT_;
return nTb.translation() + nRb * bL_ - nT_;
}

//***************************************************************************
Expand All @@ -110,9 +110,9 @@ bool GPSFactorArmCalib::equals(const NonlinearFactor& expected, double tol) cons
}

//***************************************************************************
Vector GPSFactorArmCalib::evaluateError(const Pose3& p, const Point3& bL,
Vector GPSFactorArmCalib::evaluateError(const Pose3& nTb, const Point3& bL,
OptionalMatrixType H1, OptionalMatrixType H2) const {
const Matrix3 nRb = p.rotation().matrix();
const Matrix3 nRb = nTb.rotation().matrix();
if (H1) {
H1->resize(3, 6);

Expand All @@ -124,7 +124,7 @@ Vector GPSFactorArmCalib::evaluateError(const Pose3& p, const Point3& bL,
*H2 = nRb;
}

return p.translation() + nRb * bL - nT_;
return nTb.translation() + nRb * bL - nT_;
}

//***************************************************************************
Expand All @@ -142,9 +142,9 @@ bool GPSFactor2::equals(const NonlinearFactor& expected, double tol) const {
}

//***************************************************************************
Vector GPSFactor2::evaluateError(const NavState& p,
Vector GPSFactor2::evaluateError(const NavState& nTb,
OptionalMatrixType H) const {
return p.position(H) -nT_;
return nTb.position(H) -nT_;
}

//***************************************************************************
Expand All @@ -164,9 +164,9 @@ bool GPSFactor2Arm::equals(const NonlinearFactor& expected, double tol) const {
}

//***************************************************************************
Vector GPSFactor2Arm::evaluateError(const NavState& p,
Vector GPSFactor2Arm::evaluateError(const NavState& nTb,
OptionalMatrixType H) const {
const Matrix3 nRb = p.attitude().matrix();
const Matrix3 nRb = nTb.attitude().matrix();
if (H) {
H->resize(3, 9);

Expand All @@ -175,7 +175,7 @@ Vector GPSFactor2Arm::evaluateError(const NavState& p,
H->block<3, 3>(0, 6).setZero();
}

return p.position() + nRb * bL_ - nT_;
return nTb.position() + nRb * bL_ - nT_;
}

//***************************************************************************
Expand All @@ -193,9 +193,9 @@ bool GPSFactor2ArmCalib::equals(const NonlinearFactor& expected, double tol) con
}

//***************************************************************************
Vector GPSFactor2ArmCalib::evaluateError(const NavState& p, const Point3& bL,
Vector GPSFactor2ArmCalib::evaluateError(const NavState& nTb, const Point3& bL,
OptionalMatrixType H1, OptionalMatrixType H2) const {
const Matrix3 nRb = p.attitude().matrix();
const Matrix3 nRb = nTb.attitude().matrix();
if (H1) {
H1->resize(3, 9);

Expand All @@ -208,7 +208,7 @@ Vector GPSFactor2ArmCalib::evaluateError(const NavState& p, const Point3& bL,
*H2 = nRb;
}

return p.position() + nRb * bL - nT_;
return nTb.position() + nRb * bL - nT_;
}

}/// namespace gtsam
12 changes: 6 additions & 6 deletions gtsam/navigation/GPSFactor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -83,7 +83,7 @@ class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;

/// vector of errors
Vector evaluateError(const Pose3& p, OptionalMatrixType H) const override;
Vector evaluateError(const Pose3& nTb, OptionalMatrixType H) const override;

/// return the measurement, in the navigation frame
inline const Point3 & measurementIn() const {
Expand Down Expand Up @@ -171,7 +171,7 @@ class GTSAM_EXPORT GPSFactorArm: public NoiseModelFactorN<Pose3> {
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;

/// vector of errors
Vector evaluateError(const Pose3& p, OptionalMatrixType H) const override;
Vector evaluateError(const Pose3& nTb, OptionalMatrixType H) const override;

/// return the measurement, in the navigation frame
inline const Point3 & measurementIn() const {
Expand Down Expand Up @@ -242,7 +242,7 @@ class GTSAM_EXPORT GPSFactorArmCalib: public NoiseModelFactorN<Pose3, Point3> {
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;

/// vector of errors
Vector evaluateError(const Pose3& p, const Point3& bL, OptionalMatrixType H1,
Vector evaluateError(const Pose3& nTb, const Point3& bL, OptionalMatrixType H1,
OptionalMatrixType H2) const override;

/// return the measurement, in the navigation frame
Expand Down Expand Up @@ -304,7 +304,7 @@ class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;

/// vector of errors
Vector evaluateError(const NavState& p, OptionalMatrixType H) const override;
Vector evaluateError(const NavState& nTb, OptionalMatrixType H) const override;

/// return the measurement, in the navigation frame
inline const Point3 & measurementIn() const {
Expand Down Expand Up @@ -384,7 +384,7 @@ class GTSAM_EXPORT GPSFactor2Arm: public NoiseModelFactorN<NavState> {
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;

/// vector of errors
Vector evaluateError(const NavState& p, OptionalMatrixType H) const override;
Vector evaluateError(const NavState& nTb, OptionalMatrixType H) const override;

/// return the measurement, in the navigation frame
inline const Point3 & measurementIn() const {
Expand Down Expand Up @@ -453,7 +453,7 @@ class GTSAM_EXPORT GPSFactor2ArmCalib: public NoiseModelFactorN<NavState, Point3
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;

/// vector of errors
Vector evaluateError(const NavState& p, const Point3& bL,
Vector evaluateError(const NavState& nTb, const Point3& bL,
OptionalMatrixType H1,
OptionalMatrixType H2) const override;

Expand Down
2 changes: 1 addition & 1 deletion gtsam/navigation/MagPoseFactor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -77,7 +77,7 @@ class MagPoseFactor: public NoiseModelFactorN<POSE> {
const Point& direction,
const Point& bias,
const SharedNoiseModel& model,
const std::optional<POSE>& body_P_sensor)
const std::optional<POSE>& body_P_sensor = {})
: Base(model, pose_key),
measured_(body_P_sensor ? body_P_sensor->rotation() * measured : measured),
nM_(scale * direction.normalized()),
Expand Down
4 changes: 2 additions & 2 deletions gtsam/navigation/doc/AHRSFactor.ipynb
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
},
{
"cell_type": "code",
"execution_count": 1,
"execution_count": null,
"metadata": {
"tags": [
"remove-cell"
Expand All @@ -33,7 +33,7 @@
}
],
"source": [
"%pip install --quiet gtsam plotly"
"%pip install --quiet gtsam-develop plotly"
]
},
{
Expand Down
Loading
Loading