diff --git a/gtsam/sam/tests/testBearingRangeFactor.cpp b/gtsam/sam/tests/testBearingRangeFactor.cpp index 0dcc227c72..dd0aee44e2 100644 --- a/gtsam/sam/tests/testBearingRangeFactor.cpp +++ b/gtsam/sam/tests/testBearingRangeFactor.cpp @@ -16,13 +16,12 @@ * @date July 2015 */ -#include +#include #include #include -#include #include - -#include +#include +#include using namespace std; using namespace gtsam; @@ -30,19 +29,14 @@ using namespace gtsam; namespace { Key poseKey(1); Key pointKey(2); - -typedef BearingRangeFactor BearingRangeFactor2D; -static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(2, 0.5)); -BearingRangeFactor2D factor2D(poseKey, pointKey, 1, 2, model2D); - -typedef BearingRangeFactor BearingRangeFactor3D; -static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(3, 0.5)); -BearingRangeFactor3D factor3D(poseKey, pointKey, - Pose3().bearing(Point3(1, 0, 0)), 1, model3D); -} +} // namespace /* ************************************************************************* */ TEST(BearingRangeFactor, 2D) { + typedef BearingRangeFactor BearingRangeFactor2D; + SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(2, 0.5)); + BearingRangeFactor2D factor2D(poseKey, pointKey, 1, 2, model2D); + // Set the linearization point Values values; values.insert(poseKey, Pose2(1.0, 2.0, 0.57)); @@ -54,25 +48,29 @@ TEST(BearingRangeFactor, 2D) { } /* ************************************************************************* */ -// TODO(frank): this test is disabled (for now) because the macros below are -// incompatible with the Unit3 localCoordinates. See testBearingFactor... -//TEST(BearingRangeFactor, 3D) { -// // Serialize the factor -// std::string serialized = serializeXML(factor3D); -// -// // And de-serialize it -// BearingRangeFactor3D factor; -// deserializeXML(serialized, factor); -// -// // Set the linearization point -// Values values; -// values.insert(poseKey, Pose3()); -// values.insert(pointKey, Point3(1, 0, 0)); -// -// EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression({poseKey, pointKey}), -// values, 1e-7, 1e-5); -// EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); -//} +TEST(BearingRangeFactor, 3D) { + typedef BearingRangeFactor BearingRangeFactor3D; + SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(3, 0.5)); + + const Unit3 bearing = Pose3().bearing(Point3(1, 0, 0)); + const double range = 1.0; + BearingRangeFactor3D factor3D(poseKey, pointKey, bearing, range, model3D); + + // Set the linearization point + Values values; + values.insert(poseKey, Pose3()); + values.insert(pointKey, Point3(1, 0, 0)); + + // Check that the error is zero at the linearization point + Vector actualError = factor3D.unwhitenedError(values); + EXPECT(assert_equal(Vector::Zero(actualError.size()), actualError, 1e-9)); + + // TODO(frank): this test is disabled (for now) because the macros below are + // incompatible with the Unit3 localCoordinates. See testBearingFactor... + // EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor3D.expression({poseKey, pointKey}), + // values, 1e-7, 1e-5); + // EXPECT_CORRECT_FACTOR_JACOBIANS(factor3D, values, 1e-7, 1e-5); +} // namespace /* ************************************************************************* */ int main() { diff --git a/python/gtsam/examples/BearingRange3DExample.ipynb b/python/gtsam/examples/BearingRange3DExample.ipynb new file mode 100644 index 0000000000..e7a9ca611e --- /dev/null +++ b/python/gtsam/examples/BearingRange3DExample.ipynb @@ -0,0 +1,2864 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "21db97de", + "metadata": {}, + "source": [ + "# Example of using Bearing Range in 3D\n", + "\n", + "This notebook has a very simple demonstration of locating a landmark from a moving platform. We will use a drone example." + ] + }, + { + "cell_type": "markdown", + "id": "153c8385", + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "source": [ + "GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,\n", + "Atlanta, Georgia 30332-0415\n", + "All Rights Reserved\n", + "Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n", + "\n", + "See LICENSE for the license information\n", + "\n", + "Simple robotics example using odometry measurements and bearing-range (laser) measurements\n", + "Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)" + ] + }, + { + "cell_type": "markdown", + "id": "3ced5f44", + "metadata": { + "tags": [ + "remove-input" + ] + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "id": "158d5502", + "metadata": {}, + "outputs": [], + "source": [ + "import graphviz\n", + "import numpy as np\n", + "import plotly.graph_objects as go\n", + "from gtsam.symbol_shorthand import L, X\n", + "\n", + "import gtsam\n", + "from gtsam import Point3, Pose3, Rot3" + ] + }, + { + "cell_type": "markdown", + "id": "d2980e5e", + "metadata": {}, + "source": [ + "## Setting up a Small Scenario\n", + "\n", + "It pays off to be precise about coordinate frames.\n", + "\n", + "Suppose the drone is flying straight at an altitude of 200 meters, with a velocity of 10 meters per second. We'll use an FLU (forward, left, up) body frame in an ENU (north, east, up) navigation frame. Here are three successive poses at intervals of 10 seconds:" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "id": "d32414b0", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Pose 1: R: [\n", + "\t0, -1, 0;\n", + "\t1, -0, 0;\n", + "\t0, -0, 1\n", + "]\n", + "t: 0 0 200\n", + "\n", + "Pose 2: R: [\n", + "\t0, -1, 0;\n", + "\t1, -0, 0;\n", + "\t0, -0, 1\n", + "]\n", + "t: 0 100 200\n", + "\n", + "Pose 3: R: [\n", + "\t0, -1, 0;\n", + "\t1, -0, 0;\n", + "\t0, -0, 1\n", + "]\n", + "t: 0 200 200\n", + "\n" + ] + } + ], + "source": [ + "# Set up FLU body frame in ENU nav frame, flying North:\n", + "east = Point3(1, 0, 0)\n", + "north = Point3(0, 1, 0)\n", + "up = Point3(0, 0, 1)\n", + "nRb = Rot3(north, -east, up) # body to nav\n", + "\n", + "# Generate three Pose3 objects at 10-second intervals, flying straight at 200m altitude\n", + "nT1 = Pose3(nRb, Point3(0.0, 0.0, 200.0))\n", + "nT2 = Pose3(nRb, Point3(0.0, 100.0, 200.0)) # 10 m/s * 10 s = 100 m forward\n", + "nT3 = Pose3(nRb, Point3(0.0, 200.0, 200.0)) # 10 m/s * 20 s = 200 m forward\n", + "poses = [nT1, nT2, nT3]\n", + "\n", + "print(\"Pose 1:\", nT1)\n", + "print(\"Pose 2:\", nT2)\n", + "print(\"Pose 3:\", nT3)" + ] + }, + { + "cell_type": "markdown", + "id": "5f72419e", + "metadata": {}, + "source": [ + "Now, assume a landmark is located 200 m. to the east and 100 m. to the north of our starting point:" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "id": "a7a8ca58", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Landmark: [200. 100. 0.]\n" + ] + } + ], + "source": [ + "landmark = Point3(200.0, 100.0, 0.0) # landmark at (200, 100, 0)\n", + "print(\"Landmark:\", landmark)" + ] + }, + { + "cell_type": "markdown", + "id": "426def25", + "metadata": {}, + "source": [ + "We can show the situation with plotly:" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "id": "9cc49106", + "metadata": { + "tags": [ + "remove-input" + ] + }, + "outputs": [ + { + "data": { + "application/vnd.plotly.v1+json": { + "config": { + "plotlyServerURL": "https://plot.ly" + }, + "data": [ + { + "line": { + "color": "red", + "width": 5 + }, + "mode": "lines", + "name": "Pose 1 X", + "type": "scatter3d", + "x": [ + 0, + 0 + ], + "y": [ + 0, + 30 + ], + "z": [ + 200, + 200 + ] + }, + { + "line": { + "color": "green", + "width": 5 + }, + "mode": "lines", + "name": "Pose 1 Y", + "type": "scatter3d", + "x": [ + 0, + -30 + ], + "y": [ + 0, + 0 + ], + "z": [ + 200, + 200 + ] + }, + { + "line": { + "color": "blue", + "width": 5 + }, + "mode": "lines", + "name": "Pose 1 Z", + "type": "scatter3d", + "x": [ + 0, + 0 + ], + "y": [ + 0, + 0 + ], + "z": [ + 200, + 230 + ] + }, + { + "line": { + "color": "red", + "width": 5 + }, + "mode": "lines", + "name": "Pose 2 X", + "type": "scatter3d", + "x": [ + 0, + 0 + ], + "y": [ + 100, + 130 + ], + "z": [ + 200, + 200 + ] + }, + { + "line": { + "color": "green", + "width": 5 + }, + "mode": "lines", + "name": "Pose 2 Y", + "type": "scatter3d", + "x": [ + 0, + -30 + ], + "y": [ + 100, + 100 + ], + "z": [ + 200, + 200 + ] + }, + { + "line": { + "color": "blue", + "width": 5 + }, + "mode": "lines", + "name": "Pose 2 Z", + "type": "scatter3d", + "x": [ + 0, + 0 + ], + "y": [ + 100, + 100 + ], + "z": [ + 200, + 230 + ] + }, + { + "line": { + "color": "red", + "width": 5 + }, + "mode": "lines", + "name": "Pose 3 X", + "type": "scatter3d", + "x": [ + 0, + 0 + ], + "y": [ + 200, + 230 + ], + "z": [ + 200, + 200 + ] + }, + { + "line": { + "color": "green", + "width": 5 + }, + "mode": "lines", + "name": "Pose 3 Y", + "type": "scatter3d", + "x": [ + 0, + -30 + ], + "y": [ + 200, + 200 + ], + "z": [ + 200, + 200 + ] + }, + { + "line": { + "color": "blue", + "width": 5 + }, + "mode": "lines", + "name": "Pose 3 Z", + "type": "scatter3d", + "x": [ + 0, + 0 + ], + "y": [ + 200, + 200 + ], + "z": [ + 200, + 230 + ] + }, + { + "marker": { + "color": "orange", + "size": 8, + "symbol": "diamond" + }, + "mode": "markers", + "name": "Landmark", + "type": "scatter3d", + "x": [ + 200 + ], + "y": [ + 100 + ], + "z": [ + 0 + ] + } + ], + "layout": { + "margin": { + "b": 5, + "l": 5, + "r": 5, + "t": 25 + }, + "scene": { + "aspectmode": "data", + "xaxis": { + "title": { + "text": "X (East)" + } + }, + "yaxis": { + "title": { + "text": "Y (North)" + } + }, + "zaxis": { + "title": { + "text": "Z (Up)" + } + } + }, + "showlegend": false, + "template": { + "data": { + "bar": [ + { + "error_x": { + "color": "#2a3f5f" + }, + "error_y": { + "color": "#2a3f5f" + }, + "marker": { + "line": { + "color": "#E5ECF6", + "width": 0.5 + }, + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "bar" + } + ], + "barpolar": [ + { + "marker": { + "line": { + "color": "#E5ECF6", + "width": 0.5 + }, + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "barpolar" + } + ], + "carpet": [ + { + "aaxis": { + "endlinecolor": "#2a3f5f", + "gridcolor": "white", + "linecolor": "white", + "minorgridcolor": "white", + "startlinecolor": "#2a3f5f" + }, + "baxis": { + "endlinecolor": "#2a3f5f", + "gridcolor": "white", + "linecolor": "white", + "minorgridcolor": "white", + "startlinecolor": "#2a3f5f" + }, + "type": "carpet" + } + ], + "choropleth": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "choropleth" + } + ], + "contour": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "contour" + } + ], + "contourcarpet": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "contourcarpet" + } + ], + "heatmap": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "heatmap" + } + ], + "histogram": [ + { + "marker": { + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "histogram" + } + ], + "histogram2d": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "histogram2d" + } + ], + "histogram2dcontour": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "histogram2dcontour" + } + ], + "mesh3d": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "mesh3d" + } + ], + "parcoords": [ + { + "line": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "parcoords" + } + ], + "pie": [ + { + "automargin": true, + "type": "pie" + } + ], + "scatter": [ + { + "fillpattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + }, + "type": "scatter" + } + ], + "scatter3d": [ + { + "line": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatter3d" + } + ], + "scattercarpet": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattercarpet" + } + ], + "scattergeo": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattergeo" + } + ], + "scattergl": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattergl" + } + ], + "scattermap": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattermap" + } + ], + "scattermapbox": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattermapbox" + } + ], + "scatterpolar": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterpolar" + } + ], + "scatterpolargl": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterpolargl" + } + ], + "scatterternary": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterternary" + } + ], + "surface": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "surface" + } + ], + "table": [ + { + "cells": { + "fill": { + "color": "#EBF0F8" + }, + "line": { + "color": "white" + } + }, + "header": { + "fill": { + "color": "#C8D4E3" + }, + "line": { + "color": "white" + } + }, + "type": "table" + } + ] + }, + "layout": { + "annotationdefaults": { + "arrowcolor": "#2a3f5f", + "arrowhead": 0, + "arrowwidth": 1 + }, + "autotypenumbers": "strict", + "coloraxis": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "colorscale": { + "diverging": [ + [ + 0, + "#8e0152" + ], + [ + 0.1, + "#c51b7d" + ], + [ + 0.2, + "#de77ae" + ], + [ + 0.3, + "#f1b6da" + ], + [ + 0.4, + "#fde0ef" + ], + [ + 0.5, + "#f7f7f7" + ], + [ + 0.6, + "#e6f5d0" + ], + [ + 0.7, + "#b8e186" + ], + [ + 0.8, + "#7fbc41" + ], + [ + 0.9, + "#4d9221" + ], + [ + 1, + "#276419" + ] + ], + "sequential": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "sequentialminus": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ] + }, + "colorway": [ + "#636efa", + "#EF553B", + "#00cc96", + "#ab63fa", + "#FFA15A", + "#19d3f3", + "#FF6692", + "#B6E880", + "#FF97FF", + "#FECB52" + ], + "font": { + "color": "#2a3f5f" + }, + "geo": { + "bgcolor": "white", + "lakecolor": "white", + "landcolor": "#E5ECF6", + "showlakes": true, + "showland": true, + "subunitcolor": "white" + }, + "hoverlabel": { + "align": "left" + }, + "hovermode": "closest", + "mapbox": { + "style": "light" + }, + "paper_bgcolor": "white", + "plot_bgcolor": "#E5ECF6", + "polar": { + "angularaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "bgcolor": "#E5ECF6", + "radialaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + } + }, + "scene": { + "xaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + }, + "yaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + }, + "zaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + } + }, + "shapedefaults": { + "line": { + "color": "#2a3f5f" + } + }, + "ternary": { + "aaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "baxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "bgcolor": "#E5ECF6", + "caxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + } + }, + "title": { + "x": 0.05 + }, + "xaxis": { + "automargin": true, + "gridcolor": "white", + "linecolor": "white", + "ticks": "", + "title": { + "standoff": 15 + }, + "zerolinecolor": "white", + "zerolinewidth": 2 + }, + "yaxis": { + "automargin": true, + "gridcolor": "white", + "linecolor": "white", + "ticks": "", + "title": { + "standoff": 15 + }, + "zerolinecolor": "white", + "zerolinewidth": 2 + } + } + }, + "title": { + "text": "3D Poses, Landmark, and Bearings" + } + } + } + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "def plot_pose3_axes(fig, pose: Pose3, name, axis_length=30):\n", + " \"\"\"Plot a Pose3 as three colored axes.\"\"\"\n", + " t = pose.translation()\n", + " R = pose.rotation().matrix()\n", + " x_axis = t + axis_length * R[:, 0]\n", + " y_axis = t + axis_length * R[:, 1]\n", + " z_axis = t + axis_length * R[:, 2]\n", + " # X axis (red)\n", + " fig.add_trace(go.Scatter3d(x=[t[0], x_axis[0]], y=[t[1], x_axis[1]], z=[t[2], x_axis[2]], mode='lines', line=dict(color='red', width=5), name=f'{name} X'))\n", + " fig.add_trace(go.Scatter3d(x=[t[0], y_axis[0]], y=[t[1], y_axis[1]], z=[t[2], y_axis[2]], mode='lines', line=dict(color='green', width=5), name=f'{name} Y'))\n", + " fig.add_trace(go.Scatter3d(x=[t[0], z_axis[0]], y=[t[1], z_axis[1]], z=[t[2], z_axis[2]], mode='lines', line=dict(color='blue', width=5), name=f'{name} Z'))\n", + "\n", + "fig = go.Figure()\n", + "\n", + "# Plot poses as coordinate frames\n", + "for idx, pose in enumerate(poses):\n", + " plot_pose3_axes(fig, pose, f'Pose {idx+1}')\n", + "\n", + "# Plot landmark\n", + "fig.add_trace(go.Scatter3d(\n", + " x=[landmark[0]], y=[landmark[1]], z=[landmark[2]],\n", + " mode='markers', marker=dict(size=8, color='orange', symbol='diamond'),\n", + " name='Landmark'))\n", + "\n", + "fig.update_layout(scene=dict(xaxis_title='X (East)', yaxis_title='Y (North)', zaxis_title='Z (Up)', aspectmode='data'), title='3D Poses, Landmark, and Bearings', showlegend=False)\n", + "fig.update_layout(margin=dict(l=5, r=5, t=25, b=5))\n", + "fig.show()" + ] + }, + { + "cell_type": "markdown", + "id": "48c099c0", + "metadata": {}, + "source": [ + "## Creating Bearing-Range Measurements\n", + "\n", + "Because we have the ground truth poses and landmark, we can compute the measurements we will get. In GTSAM, we actually have a handy class for such measurements, `BearingRange`. The 3D variant has a static method that can compute bearing-range given a pose and a landmark:" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "id": "1cf4edb2", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Measurements (bearing, range):\n", + "\n", + "From Pose 1:\n", + "Bearing (Unit3): [ 0.33333333 -0.66666667 -0.66666667], Range 300.0 (m)\n", + "\n", + "From Pose 2:\n", + "Bearing (Unit3): [ 0. -0.70710678 -0.70710678], Range 282.842712474619 (m)\n", + "\n", + "From Pose 3:\n", + "Bearing (Unit3): [-0.33333333 -0.66666667 -0.66666667], Range 300.0 (m)\n", + "\n" + ] + } + ], + "source": [ + "measurements = [gtsam.BearingRange3D.Measure(pose, landmark) for pose in poses]\n", + "print(\"Measurements (bearing, range):\\n\")\n", + "for i, meas in enumerate(measurements):\n", + " print(f\"From Pose {i+1}:\\nBearing (Unit3): {meas.bearing().point3()}, Range {meas.range()} (m)\\n\")" + ] + }, + { + "cell_type": "markdown", + "id": "f05d6073", + "metadata": {}, + "source": [ + "Note that the bearing measurements are of type `gtsam.Unit3`, i.e., 3D unit vectors lying on a sphere of radius $1.0$. This manifold is the right representations of directions in space.\n", + "\n", + "Here is the situation plotted. Note that we have to convert bearing vectors into nav frame, and we'll scale them with range:" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "id": "1f350495", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Sight Vectors (in nav frame):\n", + "\n", + "From Pose 1: [ 200. 100. -200.]\n", + "\n", + "From Pose 2: [ 200. 0. -200.]\n", + "\n", + "From Pose 3: [ 200. -100. -200.]\n", + "\n" + ] + } + ], + "source": [ + "sight_vectors = [meas.range()*nRb.rotate(meas.bearing().point3()) for meas in measurements]\n", + "print(\"Sight Vectors (in nav frame):\\n\")\n", + "for i, vec in enumerate(sight_vectors):\n", + " print(f\"From Pose {i+1}: {vec}\\n\")" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "id": "9a9e9bd1", + "metadata": { + "tags": [ + "remove-input" + ] + }, + "outputs": [ + { + "data": { + "application/vnd.plotly.v1+json": { + "config": { + "plotlyServerURL": "https://plot.ly" + }, + "data": [ + { + "line": { + "color": "red", + "width": 5 + }, + "mode": "lines", + "name": "Pose 1 X", + "type": "scatter3d", + "x": [ + 0, + 0 + ], + "y": [ + 0, + 30 + ], + "z": [ + 200, + 200 + ] + }, + { + "line": { + "color": "green", + "width": 5 + }, + "mode": "lines", + "name": "Pose 1 Y", + "type": "scatter3d", + "x": [ + 0, + -30 + ], + "y": [ + 0, + 0 + ], + "z": [ + 200, + 200 + ] + }, + { + "line": { + "color": "blue", + "width": 5 + }, + "mode": "lines", + "name": "Pose 1 Z", + "type": "scatter3d", + "x": [ + 0, + 0 + ], + "y": [ + 0, + 0 + ], + "z": [ + 200, + 230 + ] + }, + { + "line": { + "color": "red", + "width": 5 + }, + "mode": "lines", + "name": "Pose 2 X", + "type": "scatter3d", + "x": [ + 0, + 0 + ], + "y": [ + 100, + 130 + ], + "z": [ + 200, + 200 + ] + }, + { + "line": { + "color": "green", + "width": 5 + }, + "mode": "lines", + "name": "Pose 2 Y", + "type": "scatter3d", + "x": [ + 0, + -30 + ], + "y": [ + 100, + 100 + ], + "z": [ + 200, + 200 + ] + }, + { + "line": { + "color": "blue", + "width": 5 + }, + "mode": "lines", + "name": "Pose 2 Z", + "type": "scatter3d", + "x": [ + 0, + 0 + ], + "y": [ + 100, + 100 + ], + "z": [ + 200, + 230 + ] + }, + { + "line": { + "color": "red", + "width": 5 + }, + "mode": "lines", + "name": "Pose 3 X", + "type": "scatter3d", + "x": [ + 0, + 0 + ], + "y": [ + 200, + 230 + ], + "z": [ + 200, + 200 + ] + }, + { + "line": { + "color": "green", + "width": 5 + }, + "mode": "lines", + "name": "Pose 3 Y", + "type": "scatter3d", + "x": [ + 0, + -30 + ], + "y": [ + 200, + 200 + ], + "z": [ + 200, + 200 + ] + }, + { + "line": { + "color": "blue", + "width": 5 + }, + "mode": "lines", + "name": "Pose 3 Z", + "type": "scatter3d", + "x": [ + 0, + 0 + ], + "y": [ + 200, + 200 + ], + "z": [ + 200, + 230 + ] + }, + { + "marker": { + "color": "orange", + "size": 8, + "symbol": "diamond" + }, + "mode": "markers", + "name": "Landmark", + "type": "scatter3d", + "x": [ + 200 + ], + "y": [ + 100 + ], + "z": [ + 0 + ] + }, + { + "line": { + "color": "purple", + "dash": "dash", + "width": 4 + }, + "mode": "lines", + "name": "Bearing 1", + "type": "scatter3d", + "x": [ + 0, + 200 + ], + "y": [ + 0, + 100 + ], + "z": [ + 200, + 0 + ] + }, + { + "line": { + "color": "purple", + "dash": "dash", + "width": 4 + }, + "mode": "lines", + "name": "Bearing 2", + "type": "scatter3d", + "x": [ + 0, + 200 + ], + "y": [ + 100, + 100 + ], + "z": [ + 200, + 0 + ] + }, + { + "line": { + "color": "purple", + "dash": "dash", + "width": 4 + }, + "mode": "lines", + "name": "Bearing 3", + "type": "scatter3d", + "x": [ + 0, + 200 + ], + "y": [ + 200, + 100 + ], + "z": [ + 200, + 0 + ] + } + ], + "layout": { + "margin": { + "b": 5, + "l": 5, + "r": 5, + "t": 25 + }, + "scene": { + "aspectmode": "data", + "xaxis": { + "title": { + "text": "X (East)" + } + }, + "yaxis": { + "title": { + "text": "Y (North)" + } + }, + "zaxis": { + "title": { + "text": "Z (Up)" + } + } + }, + "showlegend": false, + "template": { + "data": { + "bar": [ + { + "error_x": { + "color": "#2a3f5f" + }, + "error_y": { + "color": "#2a3f5f" + }, + "marker": { + "line": { + "color": "#E5ECF6", + "width": 0.5 + }, + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "bar" + } + ], + "barpolar": [ + { + "marker": { + "line": { + "color": "#E5ECF6", + "width": 0.5 + }, + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "barpolar" + } + ], + "carpet": [ + { + "aaxis": { + "endlinecolor": "#2a3f5f", + "gridcolor": "white", + "linecolor": "white", + "minorgridcolor": "white", + "startlinecolor": "#2a3f5f" + }, + "baxis": { + "endlinecolor": "#2a3f5f", + "gridcolor": "white", + "linecolor": "white", + "minorgridcolor": "white", + "startlinecolor": "#2a3f5f" + }, + "type": "carpet" + } + ], + "choropleth": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "choropleth" + } + ], + "contour": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "contour" + } + ], + "contourcarpet": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "contourcarpet" + } + ], + "heatmap": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "heatmap" + } + ], + "histogram": [ + { + "marker": { + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "histogram" + } + ], + "histogram2d": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "histogram2d" + } + ], + "histogram2dcontour": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "histogram2dcontour" + } + ], + "mesh3d": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "mesh3d" + } + ], + "parcoords": [ + { + "line": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "parcoords" + } + ], + "pie": [ + { + "automargin": true, + "type": "pie" + } + ], + "scatter": [ + { + "fillpattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + }, + "type": "scatter" + } + ], + "scatter3d": [ + { + "line": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatter3d" + } + ], + "scattercarpet": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattercarpet" + } + ], + "scattergeo": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattergeo" + } + ], + "scattergl": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattergl" + } + ], + "scattermap": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattermap" + } + ], + "scattermapbox": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattermapbox" + } + ], + "scatterpolar": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterpolar" + } + ], + "scatterpolargl": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterpolargl" + } + ], + "scatterternary": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterternary" + } + ], + "surface": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "surface" + } + ], + "table": [ + { + "cells": { + "fill": { + "color": "#EBF0F8" + }, + "line": { + "color": "white" + } + }, + "header": { + "fill": { + "color": "#C8D4E3" + }, + "line": { + "color": "white" + } + }, + "type": "table" + } + ] + }, + "layout": { + "annotationdefaults": { + "arrowcolor": "#2a3f5f", + "arrowhead": 0, + "arrowwidth": 1 + }, + "autotypenumbers": "strict", + "coloraxis": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "colorscale": { + "diverging": [ + [ + 0, + "#8e0152" + ], + [ + 0.1, + "#c51b7d" + ], + [ + 0.2, + "#de77ae" + ], + [ + 0.3, + "#f1b6da" + ], + [ + 0.4, + "#fde0ef" + ], + [ + 0.5, + "#f7f7f7" + ], + [ + 0.6, + "#e6f5d0" + ], + [ + 0.7, + "#b8e186" + ], + [ + 0.8, + "#7fbc41" + ], + [ + 0.9, + "#4d9221" + ], + [ + 1, + "#276419" + ] + ], + "sequential": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "sequentialminus": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ] + }, + "colorway": [ + "#636efa", + "#EF553B", + "#00cc96", + "#ab63fa", + "#FFA15A", + "#19d3f3", + "#FF6692", + "#B6E880", + "#FF97FF", + "#FECB52" + ], + "font": { + "color": "#2a3f5f" + }, + "geo": { + "bgcolor": "white", + "lakecolor": "white", + "landcolor": "#E5ECF6", + "showlakes": true, + "showland": true, + "subunitcolor": "white" + }, + "hoverlabel": { + "align": "left" + }, + "hovermode": "closest", + "mapbox": { + "style": "light" + }, + "paper_bgcolor": "white", + "plot_bgcolor": "#E5ECF6", + "polar": { + "angularaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "bgcolor": "#E5ECF6", + "radialaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + } + }, + "scene": { + "xaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + }, + "yaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + }, + "zaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + } + }, + "shapedefaults": { + "line": { + "color": "#2a3f5f" + } + }, + "ternary": { + "aaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "baxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "bgcolor": "#E5ECF6", + "caxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + } + }, + "title": { + "x": 0.05 + }, + "xaxis": { + "automargin": true, + "gridcolor": "white", + "linecolor": "white", + "ticks": "", + "title": { + "standoff": 15 + }, + "zerolinecolor": "white", + "zerolinewidth": 2 + }, + "yaxis": { + "automargin": true, + "gridcolor": "white", + "linecolor": "white", + "ticks": "", + "title": { + "standoff": 15 + }, + "zerolinecolor": "white", + "zerolinewidth": 2 + } + } + }, + "title": { + "text": "3D Poses, Landmark, and Bearings" + } + } + } + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "fig = go.Figure()\n", + "\n", + "# Plot poses as coordinate frames\n", + "for idx, pose in enumerate(poses):\n", + " plot_pose3_axes(fig, pose, f'Pose {idx+1}')\n", + "\n", + "# Plot landmark\n", + "fig.add_trace(go.Scatter3d(\n", + " x=[landmark[0]], y=[landmark[1]], z=[landmark[2]],\n", + " mode='markers', marker=dict(size=8, color='orange', symbol='diamond'),\n", + " name='Landmark'))\n", + "\n", + "# Plot unit vectors (bearings) from each pose to the landmark\n", + "for idx, (pose, meas) in enumerate(zip(poses, measurements)):\n", + " t_k = pose.translation()\n", + " vec_end = t_k + sight_vectors[idx]\n", + " fig.add_trace(go.Scatter3d(x=[t_k[0], vec_end[0]], y=[t_k[1], vec_end[1]], z=[t_k[2], vec_end[2]], mode='lines', line=dict(color='purple', width=4, dash='dash'), name=f'Bearing {idx+1}'))\n", + "\n", + "fig.update_layout(scene=dict(xaxis_title='X (East)', yaxis_title='Y (North)', zaxis_title='Z (Up)', aspectmode='data'), title='3D Poses, Landmark, and Bearings', showlegend=False)\n", + "fig.update_layout(margin=dict(l=5, r=5, t=25, b=5))\n", + "fig.show()" + ] + }, + { + "cell_type": "markdown", + "id": "060574f1", + "metadata": {}, + "source": [ + "## Sensor Fusion with GTSAM\n", + "\n", + "We assume the drone has a navigation system that gives us `Pose3` updates, with a covariance. We create a factor graph with three prior factors to provide that information:" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "id": "cb3d5141", + "metadata": {}, + "outputs": [], + "source": [ + "# Create a NonlinearFactorGraph with three Pose3 prior factors at different locations.\n", + "graph3d = gtsam.NonlinearFactorGraph()\n", + "\n", + "sigma_degree = 5\n", + "sigma_meter = 0.5\n", + "sigmas:np.ndarray = np.array([np.radians(sigma_degree)] * 3 + [sigma_meter] * 3)\n", + "PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(sigmas)\n", + "for idx, nTk in enumerate(poses):\n", + " graph3d.addPriorPose3(X(idx+1), nTk, PRIOR_NOISE)" + ] + }, + { + "cell_type": "markdown", + "id": "b9efa777", + "metadata": {}, + "source": [ + "We then have three bearing range factors, created like this" + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "id": "1301ff6a", + "metadata": {}, + "outputs": [], + "source": [ + "# Create a noise model (isotropic, 3D, sigma=0.5)\n", + "model3D = gtsam.noiseModel.Isotropic.Sigma(3, 0.5)\n", + "\n", + "# Create the 3D bearing-range factors\n", + "for idx, meas in enumerate(measurements):\n", + " factor = gtsam.BearingRangeFactor3D(X(idx+1), L(1), meas.bearing(), meas.range(), model3D)\n", + " graph3d.add(factor)" + ] + }, + { + "cell_type": "markdown", + "id": "080524e8", + "metadata": {}, + "source": [ + "We wil set up an optimization problem starting from *completely wrong* values:" + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "id": "40089be2", + "metadata": {}, + "outputs": [], + "source": [ + "# Set up values at the linearization point\n", + "initial_estimate = gtsam.Values()\n", + "for k, _ in enumerate(poses, 1):\n", + " initial_estimate.insert(X(k), Pose3())\n", + "initial_estimate.insert(L(1), Point3(1,2,3))" + ] + }, + { + "cell_type": "code", + "execution_count": 11, + "id": "d896ecee", + "metadata": {}, + "outputs": [ + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var7782220156096217089\n", + "\n", + "l1\n", + "\n", + "\n", + "\n", + "factor3\n", + "\n", + "\n", + "\n", + "\n", + "var7782220156096217089--factor3\n", + "\n", + "\n", + "\n", + "\n", + "factor4\n", + "\n", + "\n", + "\n", + "\n", + "var7782220156096217089--factor4\n", + "\n", + "\n", + "\n", + "\n", + "factor5\n", + "\n", + "\n", + "\n", + "\n", + "var7782220156096217089--factor5\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321\n", + "\n", + "x1\n", + "\n", + "\n", + "\n", + "factor0\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor0\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor3\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352322\n", + "\n", + "x2\n", + "\n", + "\n", + "\n", + "factor1\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352322--factor1\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352322--factor4\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352323\n", + "\n", + "x3\n", + "\n", + "\n", + "\n", + "factor2\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352323--factor2\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352323--factor5\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "display(graphviz.Source(graph3d.dot(initial_estimate)))" + ] + }, + { + "cell_type": "markdown", + "id": "2608bf96", + "metadata": {}, + "source": [ + "Now, we use an optimizer to find the variable configuration that best fits all the factors (measurements) in the graph, starting from the initial estimate.\n", + "\n", + "We'll use the Levenberg-Marquardt (LM) algorithm, a standard non-linear least-squares optimizer.\n", + "\n", + "1. Create LM parameters (`gtsam.LevenbergMarquardtParams`). We'll use the defaults.\n", + "2. Create the optimizer instance, providing the graph, initial estimate, and parameters.\n", + "3. Run the optimization by calling `optimizer.optimize()`." + ] + }, + { + "cell_type": "code", + "execution_count": 12, + "id": "2ee6b17a", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Initial error: 870772, values: 4\n", + "iter cost cost_change lambda success iter_time\n", + " 0 29705 8.4e+05 1e-05 1 0\n", + " 1 1.6e+07 -1.6e+07 1e-06 1 0\n", + " 1 1.4e+07 -1.4e+07 1e-05 1 0\n", + " 1 6.1e+06 -6.1e+06 0.0001 1 0\n", + " 1 6.1e+05 -5.8e+05 0.001 1 0\n", + " 1 1.8e+05 -1.5e+05 0.01 1 0\n", + " 1 7.3e+03 2.2e+04 0.1 1 0\n", + " 2 2.5e+02 7e+03 0.01 1 0\n", + " 3 1.1e+02 1.4e+02 0.001 1 0\n", + " 4 3e+04 -3e+04 0.0001 1 0\n", + " 4 73 41 0.001 1 0\n", + " 5 2e+04 -2e+04 0.0001 1 0\n", + " 5 48 25 0.001 1 0\n", + " 6 1.3e+04 -1.3e+04 0.0001 1 0\n", + " 6 32 17 0.001 1 0\n", + " 7 8.3e+03 -8.3e+03 0.0001 1 0\n", + " 7 21 11 0.001 1 0\n", + " 8 5.3e+03 -5.2e+03 0.0001 1 0\n", + " 8 13 7.2 0.001 1 0\n", + " 9 3.3e+03 -3.3e+03 0.0001 1 0\n", + " 9 8.7 4.6 0.001 1 0\n", + " 10 2e+03 -2e+03 0.0001 1 0\n", + " 10 5.7 3 0.001 1 0\n", + " 11 1.3e+03 -1.3e+03 0.0001 1 0\n", + " 11 3.8 1.9 0.001 1 0\n", + " 12 7.8e+02 -7.7e+02 0.0001 1 0\n", + " 12 2.6 1.2 0.001 1 0\n", + " 13 4.7e+02 -4.7e+02 0.0001 1 0\n", + " 13 1.7 0.82 0.001 1 0\n", + " 14 2.9e+02 -2.9e+02 0.0001 1 0\n", + " 14 1.2 0.54 0.001 1 0\n", + " 15 1.8e+02 -1.7e+02 0.0001 1 0\n", + " 15 0.84 0.36 0.001 1 0\n", + " 16 1.1e+02 -1.1e+02 0.0001 1 0\n", + " 16 0.6 0.24 0.001 1 0\n", + " 17 65 -64 0.0001 1 0\n", + " 17 0.43 0.17 0.001 1 0\n", + " 18 39 -39 0.0001 1 0\n", + " 18 0.31 0.12 0.001 1 0\n", + " 19 24 -23 0.0001 1 0\n", + " 19 0.23 0.082 0.001 1 0\n", + " 20 14 -14 0.0001 1 0\n", + " 20 0.17 0.059 0.001 1 0\n", + " 21 8.7 -8.5 0.0001 1 0\n", + " 21 0.13 0.043 0.001 1 0\n", + " 22 5.3 -5.1 0.0001 1 0\n", + " 22 0.097 0.031 0.001 1 0\n", + " 23 3.2 -3.1 0.0001 1 0\n", + " 23 0.074 0.023 0.001 1 0\n", + " 24 1.9 -1.8 0.0001 1 0\n", + " 24 0.056 0.017 0.001 1 0\n", + " 25 1.2 -1.1 0.0001 1 0\n", + " 25 0.043 0.013 0.001 1 0\n", + " 26 0.7 -0.66 0.0001 1 0\n", + " 26 0.033 0.01 0.001 1 0\n", + " 27 0.42 -0.39 0.0001 1 0\n", + " 27 0.025 0.0076 0.001 1 0\n", + " 28 0.26 -0.23 0.0001 1 0\n", + " 28 0.02 0.0058 0.001 1 0\n", + " 29 0.16 -0.14 0.0001 1 0\n", + " 29 0.015 0.0045 0.001 1 0\n", + " 30 0.095 -0.079 0.0001 1 0\n", + " 30 0.012 0.0034 0.001 1 0\n", + " 31 0.058 -0.046 0.0001 1 0\n", + " 31 0.0091 0.0026 0.001 1 0\n", + " 32 0.035 -0.026 0.0001 1 0\n", + " 32 0.007 0.002 0.001 1 0\n", + " 33 0.021 -0.014 0.0001 1 0\n", + " 33 0.0055 0.0016 0.001 1 0\n", + " 34 0.013 -0.0077 0.0001 1 0\n", + " 34 0.0042 0.0012 0.001 1 0\n", + " 35 0.0081 -0.0039 0.0001 1 0\n", + " 35 0.0033 0.00095 0.001 1 0\n", + " 36 0.005 -0.0017 0.0001 1 0\n", + " 36 0.0025 0.00073 0.001 1 0\n", + " 37 0.0031 -0.00058 0.0001 1 0\n", + " 37 0.002 0.00057 0.001 1 0\n", + " 38 0.002 7.4e-06 0.0001 1 0\n" + ] + } + ], + "source": [ + "# Optimize using Levenberg-Marquardt optimization.\n", + "# The optimizer accepts optional parameters, but we'll use the defaults here.\n", + "params = gtsam.LevenbergMarquardtParams()\n", + "params.setVerbosityLM(\"SUMMARY\")\n", + "optimizer = gtsam.LevenbergMarquardtOptimizer(graph3d, initial_estimate, params)\n", + "\n", + "# Perform the optimization\n", + "result = optimizer.optimize()" + ] + }, + { + "cell_type": "markdown", + "id": "6111efa9", + "metadata": {}, + "source": [ + "As you can see, and not surprising as we used the ground truth measurements without adding noise, the error is optimized to zero. Note this is a very non-linear objective function, and hence it takes a few iterations to converge. However, this would be much faster with a good initial estimate.\n", + "\n", + "The recovered poses and landmarks are here:" + ] + }, + { + "cell_type": "code", + "execution_count": 13, + "id": "1864e025", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Final Result:\n", + "Values with 4 values:\n", + "Value l1: (Eigen::Matrix)\n", + "[\n", + "\t2e+02;\n", + "\t1e+02;\n", + "\t1.6\n", + "]\n", + "\n", + "Value x1: (gtsam::Pose3)\n", + "R: [\n", + "\t5.4e-05, -1, -0.00021;\n", + "\t1, 5.4e-05, -5.3e-05;\n", + "\t5.3e-05, -0.00021, 1\n", + "]\n", + "t: 1.7e-05 -1.9e-07 2e+02\n", + "\n", + "Value x2: (gtsam::Pose3)\n", + "R: [\n", + "\t-1.1e-16, -1, -0.00024;\n", + "\t1, 1e-16, -7.2e-18;\n", + "\t2.5e-19, -0.00024, 1\n", + "]\n", + "t: 1.9e-05 1e+02 2e+02\n", + "\n", + "Value x3: (gtsam::Pose3)\n", + "R: [\n", + "\t-5.4e-05, -1, -0.00021;\n", + "\t1, -5.4e-05, 5.3e-05;\n", + "\t-5.3e-05, -0.00021, 1\n", + "]\n", + "t: 1.7e-05 2e+02 2e+02\n", + "\n", + "\n" + ] + } + ], + "source": [ + "# Print the final optimized result\n", + "# This gtsam.Values object contains the most likely estimates for all variables.\n", + "print(\"\\nFinal Result:\\n{}\".format(result))" + ] + }, + { + "cell_type": "markdown", + "id": "74673b3d", + "metadata": {}, + "source": [ + "## Summary\n", + "\n", + "We have successfully:\n", + "1. Set up a small ground truth scenario.\n", + "2. Created (exact) bearing range measurements, in the body frame.\n", + "3. Showed th situation graphically, after rotating bearing-range vectors into nav.\n", + "3. Represented this problem as a `gtsam.NonlinearFactorGraph`.\n", + "4. Provided a very wrong initial estimate.\n", + "5. Used `gtsam.LevenbergMarquardtOptimizer` to recover the exact ground truth.\n", + "\n", + "Hopefully this is useful in using bearing-range-like sensors in your own robotics scenario." + ] + } + ], + "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": 5 +}