Skip to content

Commit 967c3ed

Browse files
dellaertdellaert
authored
Merge pull request #2084 from borglab/feature/more_nav_docs
More navigation docs
2 parents c2f9c5e + 5eaba68 commit 967c3ed

22 files changed

+3277
-76
lines changed

‎gtsam/navigation/ConstantVelocityFactor.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -41,7 +41,7 @@ class ConstantVelocityFactor : public NoiseModelFactorN<NavState, NavState> {
4141
~ConstantVelocityFactor() override {}
4242

4343
/**
44-
* @brief Caclulate error: (x2 - x1.update(dt)))
44+
* @brief Calculate error: (x2 - x1.update(dt)))
4545
* where X1 and X1 are NavStates and dt is
4646
* the time difference in seconds between the states.
4747
* @param x1 NavState for key a

‎gtsam/navigation/GPSFactor.cpp‎

Lines changed: 16 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -37,9 +37,9 @@ bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const {
3737
}
3838

3939
//***************************************************************************
40-
Vector GPSFactor::evaluateError(const Pose3& p,
40+
Vector GPSFactor::evaluateError(const Pose3& nTb,
4141
OptionalMatrixType H) const {
42-
return p.translation(H) -nT_;
42+
return nTb.translation(H) -nT_;
4343
}
4444

4545
//***************************************************************************
@@ -82,17 +82,17 @@ bool GPSFactorArm::equals(const NonlinearFactor& expected, double tol) const {
8282
}
8383

8484
//***************************************************************************
85-
Vector GPSFactorArm::evaluateError(const Pose3& p,
85+
Vector GPSFactorArm::evaluateError(const Pose3& nTb,
8686
OptionalMatrixType H) const {
87-
const Matrix3 nRb = p.rotation().matrix();
87+
const Matrix3 nRb = nTb.rotation().matrix();
8888
if (H) {
8989
H->resize(3, 6);
9090

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

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

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

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

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

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

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

144144
//***************************************************************************
145-
Vector GPSFactor2::evaluateError(const NavState& p,
145+
Vector GPSFactor2::evaluateError(const NavState& nTb,
146146
OptionalMatrixType H) const {
147-
return p.position(H) -nT_;
147+
return nTb.position(H) -nT_;
148148
}
149149

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

166166
//***************************************************************************
167-
Vector GPSFactor2Arm::evaluateError(const NavState& p,
167+
Vector GPSFactor2Arm::evaluateError(const NavState& nTb,
168168
OptionalMatrixType H) const {
169-
const Matrix3 nRb = p.attitude().matrix();
169+
const Matrix3 nRb = nTb.attitude().matrix();
170170
if (H) {
171171
H->resize(3, 9);
172172

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

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

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

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

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

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

214214
}/// namespace gtsam

‎gtsam/navigation/GPSFactor.h‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -83,7 +83,7 @@ class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
8383
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
8484

8585
/// vector of errors
86-
Vector evaluateError(const Pose3& p, OptionalMatrixType H) const override;
86+
Vector evaluateError(const Pose3& nTb, OptionalMatrixType H) const override;
8787

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

173173
/// vector of errors
174-
Vector evaluateError(const Pose3& p, OptionalMatrixType H) const override;
174+
Vector evaluateError(const Pose3& nTb, OptionalMatrixType H) const override;
175175

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

244244
/// vector of errors
245-
Vector evaluateError(const Pose3& p, const Point3& bL, OptionalMatrixType H1,
245+
Vector evaluateError(const Pose3& nTb, const Point3& bL, OptionalMatrixType H1,
246246
OptionalMatrixType H2) const override;
247247

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

306306
/// vector of errors
307-
Vector evaluateError(const NavState& p, OptionalMatrixType H) const override;
307+
Vector evaluateError(const NavState& nTb, OptionalMatrixType H) const override;
308308

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

386386
/// vector of errors
387-
Vector evaluateError(const NavState& p, OptionalMatrixType H) const override;
387+
Vector evaluateError(const NavState& nTb, OptionalMatrixType H) const override;
388388

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

455455
/// vector of errors
456-
Vector evaluateError(const NavState& p, const Point3& bL,
456+
Vector evaluateError(const NavState& nTb, const Point3& bL,
457457
OptionalMatrixType H1,
458458
OptionalMatrixType H2) const override;
459459

‎gtsam/navigation/MagPoseFactor.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -77,7 +77,7 @@ class MagPoseFactor: public NoiseModelFactorN<POSE> {
7777
const Point& direction,
7878
const Point& bias,
7979
const SharedNoiseModel& model,
80-
const std::optional<POSE>& body_P_sensor)
80+
const std::optional<POSE>& body_P_sensor = {})
8181
: Base(model, pose_key),
8282
measured_(body_P_sensor ? body_P_sensor->rotation() * measured : measured),
8383
nM_(scale * direction.normalized()),

‎gtsam/navigation/doc/AHRSFactor.ipynb‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,7 @@
1717
},
1818
{
1919
"cell_type": "code",
20-
"execution_count": 1,
20+
"execution_count": null,
2121
"metadata": {
2222
"tags": [
2323
"remove-cell"
@@ -33,7 +33,7 @@
3333
}
3434
],
3535
"source": [
36-
"%pip install --quiet gtsam plotly"
36+
"%pip install --quiet gtsam-develop plotly"
3737
]
3838
},
3939
{

0 commit comments

Comments
 (0)