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