GPSFactor + wrapping
parent
12104b779c
commit
ae87ca690d
|
@ -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_;
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
|
@ -82,9 +82,9 @@ 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);
|
||||
|
||||
|
@ -92,7 +92,7 @@ Vector GPSFactorArm::evaluateError(const Pose3& p,
|
|||
H->block<3, 3>(0, 3) = nRb;
|
||||
}
|
||||
|
||||
return p.translation() + nRb * bL_ - nT_;
|
||||
return nTb.translation() + nRb * bL_ - nT_;
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
|
@ -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);
|
||||
|
||||
|
@ -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_;
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
|
@ -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_;
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
|
@ -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);
|
||||
|
||||
|
@ -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_;
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
|
@ -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);
|
||||
|
||||
|
@ -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
|
||||
|
|
|
@ -83,7 +83,7 @@ public:
|
|||
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 {
|
||||
|
@ -171,7 +171,7 @@ public:
|
|||
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 {
|
||||
|
@ -242,7 +242,7 @@ public:
|
|||
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
|
||||
|
@ -304,7 +304,7 @@ public:
|
|||
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 {
|
||||
|
@ -384,7 +384,7 @@ public:
|
|||
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 {
|
||||
|
@ -453,7 +453,7 @@ public:
|
|||
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;
|
||||
|
||||
|
|
|
@ -0,0 +1,244 @@
|
|||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"# GPSFactor Family\n",
|
||||
"\n",
|
||||
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/GPSFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>\n",
|
||||
"\n",
|
||||
"## Overview\n",
|
||||
"\n",
|
||||
"The `GPSFactor` family provides factors for incorporating Global Positioning System (GPS) measurements into a GTSAM factor graph. GPS typically provides measurements of position in Latitude/Longitude/Height. These GPS factors, however, assume the GPS measurement has been converted into a local Cartesian **navigation frame** (e.g., [ENU, NED, or ECEF](https://dirsig.cis.rit.edu/docs/new/coordinates.html)).\n",
|
||||
"\n",
|
||||
"Different variants exist to handle:\n",
|
||||
"- State type: `Pose3` or `NavState`.\n",
|
||||
"- Lever arm: Whether the GPS antenna is offset from the body frame origin.\n",
|
||||
"- Calibration: Whether the lever arm itself is being estimated."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {
|
||||
"tags": [
|
||||
"remove-cell"
|
||||
]
|
||||
},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"%pip install --quiet gtsam-develop"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## Factor Variants\n",
|
||||
"\n",
|
||||
"**For `Pose3` states:**\n",
|
||||
"\n",
|
||||
"- **`GPSFactor`**: \n",
|
||||
" - Connects to: `Pose3` key.\n",
|
||||
" - Assumes: Zero lever arm (GPS measurement corresponds directly to the `Pose3` origin).\n",
|
||||
" - Measurement: 3D position (`Point3`) in the navigation frame.\n",
|
||||
" - Error: $p_{pose} - p_{measured}$\n",
|
||||
"\n",
|
||||
"- **`GPSFactorArm`**: \n",
|
||||
" - Connects to: `Pose3` key.\n",
|
||||
" - Assumes: Known, fixed lever arm (`bL` vector in the body frame).\n",
|
||||
" - Measurement: 3D position (`Point3`) in the navigation frame.\n",
|
||||
" - Error: $(p_{pose} + R_{nb} \\cdot bL) - p_{measured}$\n",
|
||||
"\n",
|
||||
"- **`GPSFactorArmCalib`**: \n",
|
||||
" - Connects to: `Pose3` key, `Point3` key (for the lever arm).\n",
|
||||
" - Assumes: Lever arm (`bL`) is unknown and estimated.\n",
|
||||
" - Measurement: 3D position (`Point3`) in the navigation frame.\n",
|
||||
" - Error: $(p_{pose} + R_{nb} \\cdot bL_{estimated}) - p_{measured}$\n",
|
||||
"\n",
|
||||
"**For `NavState` states:**\n",
|
||||
"\n",
|
||||
"- **`GPSFactor2`**: Like `GPSFactor` but connects to a `NavState` key.\n",
|
||||
"- **`GPSFactor2Arm`**: Like `GPSFactorArm` but connects to a `NavState` key.\n",
|
||||
"- **`GPSFactor2ArmCalib`**: Like `GPSFactorArmCalib` but connects to a `NavState` key and a `Point3` lever arm key.\n",
|
||||
"\n",
|
||||
"(The '2' suffix historically denoted factors using `NavState`)."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## Mathematical Formulation (GPSFactorArm Example)\n",
|
||||
"\n",
|
||||
"Let:\n",
|
||||
"- $T_{nb} = (R_{nb}, p_{nb})$ be the `Pose3` state (body frame $b$ in navigation frame $n$).\n",
|
||||
"- $L_b$ be the known lever arm vector from the body origin to the GPS antenna, expressed in the body frame.\n",
|
||||
"- $p_{gps}$ be the measured GPS position in the navigation frame.\n",
|
||||
"\n",
|
||||
"The predicted position of the GPS antenna in the navigation frame is:\n",
|
||||
"$$ p_{ant, pred} = p_{nb} + R_{nb} \\cdot L_b $$ \n",
|
||||
"\n",
|
||||
"The factor's 3D error vector is the difference between the predicted antenna position and the measured GPS position:\n",
|
||||
"$$ e = p_{ant, pred} - p_{gps} $$ \n",
|
||||
"\n",
|
||||
"The noise model reflects the uncertainty of the $p_{gps}$ measurement in the navigation frame."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## Key Functionality / API (Common Patterns)\n",
|
||||
"\n",
|
||||
"- **Constructor**: Takes the relevant key(s), the measured `Point3` position `gpsIn` (in nav frame), the noise model, and potentially the `leverArm` (`Point3` in body frame).\n",
|
||||
"- **`evaluateError(...)`**: Calculates the 3D error vector based on the connected state variable(s) and the measurement.\n",
|
||||
"- **`measurementIn()`**: Returns the stored `Point3` measurement.\n",
|
||||
"- **`leverArm()`** (For Arm variants): Returns the stored `Point3` lever arm."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## Usage Example (GPSFactor and GPSFactorArm)\n",
|
||||
"\n",
|
||||
"Assume we have a GPS reading converted to a local ENU frame."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 1,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"Created GPSFactor (zero lever arm):\n",
|
||||
"GPSFactor on x0\n",
|
||||
" GPS measurement: 10.5\n",
|
||||
"20.2\n",
|
||||
" 5.1\n",
|
||||
" noise model: diagonal sigmas [0.5; 0.5; 1];\n",
|
||||
"\n",
|
||||
"GPSFactor Error: [-0.5 -0.2 -0.1]\n",
|
||||
"\n",
|
||||
"Created GPSFactorArm:\n",
|
||||
"GPSFactorArm on x0\n",
|
||||
" GPS measurement: 10.5 20.2 5.1\n",
|
||||
" Lever arm: -0.1 0 0.05\n",
|
||||
" noise model: diagonal sigmas [0.5; 0.5; 1];\n",
|
||||
"\n",
|
||||
"GPSFactorArm Error: [-0.6 -0.2 -0.05]\n",
|
||||
" ( Pose: [10. 20. 5.] )\n",
|
||||
" ( Lever Arm: [-0.1 0. 0.05] )\n",
|
||||
" ( Predicted Antenna Pos: [ 9.9 20. 5.05] )\n",
|
||||
" ( Measured GPS Pos: [10.5 20.2 5.1] )\n",
|
||||
"\n",
|
||||
"Created GPSFactorArmCalib:\n",
|
||||
"GPSFactorArmCalib on x0\n",
|
||||
" GPS measurement: 10.5 20.2 5.1\n",
|
||||
" noise model: diagonal sigmas [0.5; 0.5; 1];\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"import gtsam\n",
|
||||
"import numpy as np\n",
|
||||
"from gtsam.symbol_shorthand import X, L # Pose key, Lever arm key\n",
|
||||
"\n",
|
||||
"# --- Setup ---\n",
|
||||
"pose_key = X(0)\n",
|
||||
"\n",
|
||||
"# GPS Measurement in local ENU frame (meters)\n",
|
||||
"gps_measurement_enu = gtsam.Point3(10.5, 20.2, 5.1)\n",
|
||||
"\n",
|
||||
"# Noise model for GPS measurement (e.g., 0.5m horizontal, 1.0m vertical sigma)\n",
|
||||
"gps_sigmas = np.array([0.5, 0.5, 1.0])\n",
|
||||
"gps_noise_model = gtsam.noiseModel.Diagonal.Sigmas(gps_sigmas)\n",
|
||||
"\n",
|
||||
"# --- Scenario 1: GPSFactor (Zero Lever Arm) ---\n",
|
||||
"gps_factor_zero_arm = gtsam.GPSFactor(pose_key, gps_measurement_enu, gps_noise_model)\n",
|
||||
"print(\"Created GPSFactor (zero lever arm):\")\n",
|
||||
"gps_factor_zero_arm.print()\n",
|
||||
"\n",
|
||||
"# Evaluate error: Error is difference between pose translation and measurement\n",
|
||||
"test_pose1 = gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(10.0, 20.0, 5.0))\n",
|
||||
"error1 = gps_factor_zero_arm.evaluateError(test_pose1)\n",
|
||||
"print(\"\\nGPSFactor Error:\", error1) # Expected: [0.5, 0.2, 0.1]\n",
|
||||
"\n",
|
||||
"# --- Scenario 2: GPSFactorArm (Known Lever Arm) ---\n",
|
||||
"# Assume antenna is 10cm behind and 5cm above the body origin\n",
|
||||
"lever_arm_body = gtsam.Point3(-0.1, 0.0, 0.05) \n",
|
||||
"\n",
|
||||
"gps_factor_with_arm = gtsam.GPSFactorArm(pose_key, gps_measurement_enu, \n",
|
||||
" lever_arm_body, gps_noise_model)\n",
|
||||
"print(\"\\nCreated GPSFactorArm:\")\n",
|
||||
"gps_factor_with_arm.print()\n",
|
||||
"\n",
|
||||
"# Evaluate error: Error is difference between (pose + R*lever_arm) and measurement\n",
|
||||
"# Use the same test pose as before\n",
|
||||
"predicted_antenna_pos = test_pose1.transformFrom(lever_arm_body)\n",
|
||||
"error2 = gps_factor_with_arm.evaluateError(test_pose1)\n",
|
||||
"print(\"\\nGPSFactorArm Error:\", error2) \n",
|
||||
"print(\" ( Pose: \", test_pose1.translation() , \")\")\n",
|
||||
"print(\" ( Lever Arm: \", lever_arm_body, \")\")\n",
|
||||
"print(\" ( Predicted Antenna Pos: \", predicted_antenna_pos, \")\")\n",
|
||||
"print(\" ( Measured GPS Pos: \", gps_measurement_enu, \")\")\n",
|
||||
"# Expected: predicted_antenna_pos - gps_measurement_enu \n",
|
||||
"# = [9.9, 20.0, 5.05] - [10.5, 20.2, 5.1] = [-0.6, -0.2, -0.05]\n",
|
||||
"\n",
|
||||
"# --- Scenario 3: GPSFactorArmCalib (Example Setup - Not Evaluated) ---\n",
|
||||
"lever_arm_key = L(0) # Key for the unknown lever arm\n",
|
||||
"gps_factor_calib = gtsam.GPSFactorArmCalib(pose_key, lever_arm_key, \n",
|
||||
" gps_measurement_enu, gps_noise_model)\n",
|
||||
"print(\"\\nCreated GPSFactorArmCalib:\")\n",
|
||||
"gps_factor_calib.print()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## Coordinate Frames\n",
|
||||
"\n",
|
||||
"It's crucial to ensure consistency in coordinate frames:\n",
|
||||
"- **GPS Measurement (`gpsIn`)**: Must be provided in the chosen local Cartesian **navigation frame** (e.g., ENU, NED).\n",
|
||||
"- **Lever Arm (`leverArm`)**: Must be provided in the **body frame**.\n",
|
||||
"- **Pose/NavState Variables**: Represent the pose of the body frame in the navigation frame ($T_{nb}$)."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## Source\n",
|
||||
"- [GPSFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/GPSFactor.h)\n",
|
||||
"- [GPSFactor.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/GPSFactor.cpp)"
|
||||
]
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
"kernelspec": {
|
||||
"display_name": "py312",
|
||||
"language": "python",
|
||||
"name": "python3"
|
||||
},
|
||||
"language_info": {
|
||||
"codemirror_mode": {
|
||||
"name": "ipython",
|
||||
"version": 3
|
||||
},
|
||||
"file_extension": ".py",
|
||||
"mimetype": "text/x-python",
|
||||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.12.6"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
"nbformat_minor": 2
|
||||
}
|
|
@ -388,6 +388,41 @@ virtual class GPSFactor : gtsam::NonlinearFactor{
|
|||
void serialize() const;
|
||||
};
|
||||
|
||||
virtual class GPSFactorArm : gtsam::NonlinearFactor{
|
||||
GPSFactorArm(size_t key, const gtsam::Point3& gpsIn,
|
||||
const gtsam::Point3& leverArm,
|
||||
const gtsam::noiseModel::Base* model);
|
||||
|
||||
// Testable
|
||||
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
|
||||
gtsam::DefaultKeyFormatter) const;
|
||||
bool equals(const gtsam::NonlinearFactor& expected, double tol);
|
||||
|
||||
// Standard Interface
|
||||
gtsam::Point3 measurementIn() const;
|
||||
gtsam::Vector evaluateError(const gtsam::Pose3& nTb);
|
||||
|
||||
// enable serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
virtual class GPSFactorArmCalib : gtsam::NonlinearFactor{
|
||||
GPSFactorArmCalib(size_t key1, size_t key2, const gtsam::Point3& gpsIn,
|
||||
const gtsam::noiseModel::Base* model);
|
||||
|
||||
// Testable
|
||||
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
|
||||
gtsam::DefaultKeyFormatter) const;
|
||||
bool equals(const gtsam::NonlinearFactor& expected, double tol);
|
||||
|
||||
// Standard Interface
|
||||
gtsam::Point3 measurementIn() const;
|
||||
gtsam::Vector evaluateError(const gtsam::Pose3& nTb, const gtsam::Point3& leverArm);
|
||||
|
||||
// enable serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
virtual class GPSFactor2 : gtsam::NonlinearFactor {
|
||||
GPSFactor2(size_t key, const gtsam::Point3& gpsIn,
|
||||
const gtsam::noiseModel::Base* model);
|
||||
|
@ -405,6 +440,41 @@ virtual class GPSFactor2 : gtsam::NonlinearFactor {
|
|||
void serialize() const;
|
||||
};
|
||||
|
||||
virtual class GPSFactor2Arm : gtsam::NonlinearFactor{
|
||||
GPSFactor2Arm(size_t key, const gtsam::Point3& gpsIn,
|
||||
const gtsam::Point3& leverArm,
|
||||
const gtsam::noiseModel::Base* model);
|
||||
|
||||
// Testable
|
||||
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
|
||||
gtsam::DefaultKeyFormatter) const;
|
||||
bool equals(const gtsam::NonlinearFactor& expected, double tol);
|
||||
|
||||
// Standard Interface
|
||||
gtsam::Point3 measurementIn() const;
|
||||
gtsam::Vector evaluateError(const gtsam::NavState& nTb);
|
||||
|
||||
// enable serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
virtual class GPSFactor2ArmCalib : gtsam::NonlinearFactor{
|
||||
GPSFactor2ArmCalib(size_t key1, size_t key2, const gtsam::Point3& gpsIn,
|
||||
const gtsam::noiseModel::Base* model);
|
||||
|
||||
// Testable
|
||||
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
|
||||
gtsam::DefaultKeyFormatter) const;
|
||||
bool equals(const gtsam::NonlinearFactor& expected, double tol);
|
||||
|
||||
// Standard Interface
|
||||
gtsam::Point3 measurementIn() const;
|
||||
gtsam::Vector evaluateError(const gtsam::NavState& nTb, const gtsam::Point3& leverArm);
|
||||
|
||||
// enable serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
#include <gtsam/navigation/BarometricFactor.h>
|
||||
virtual class BarometricFactor : gtsam::NonlinearFactor {
|
||||
BarometricFactor();
|
||||
|
|
Loading…
Reference in New Issue