From adfa375a7fb353656dd4f565d4990fa8fe6f064b Mon Sep 17 00:00:00 2001 From: Dehann Fourie <6412556+dehann@users.noreply.github.com> Date: Tue, 29 Mar 2022 23:41:38 -0700 Subject: [PATCH 1/4] Update icra-1-simple.json --- julia/navability-sdk/icra-1-simple.json | 576 +++++++++++++++++++++++- 1 file changed, 575 insertions(+), 1 deletion(-) diff --git a/julia/navability-sdk/icra-1-simple.json b/julia/navability-sdk/icra-1-simple.json index 77356c3..ab852d5 100644 --- a/julia/navability-sdk/icra-1-simple.json +++ b/julia/navability-sdk/icra-1-simple.json @@ -1 +1,575 @@ -test +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Tutorial 1: Creating and Solving Factor Graphs\n", + "\n", + "## API Version: NavAbilitySDK.jl\n", + "\n", + "In this tutorial you will learn how to create, solve, and explore factor graphs, as well as what variables, factors, and priors are.\n", + "\n", + "## What is a Factor Graph\n", + "\n", + "Wikipedia tells us a factor graph is a bipartite graph representing the factorization of a function.\n", + " \n", + "If you haven't taken some advanced statistics classes you are probably wondering: Bipartite graph? Factorization? I just want to navigate my robot.\n", + "For robotics, factor graphs are the common language used to describe your robot's estimation problem (the function) in a way both humans and computers can understand. \n", + "The estimation problem can be anything from robot localization, structure from motion, calibration to full SLAM with parametric or non-parametric measurements and beliefs.\n", + "A factor graph is a graphical model with two types of nodes, variables and factors, connected by edges between the variables and factors (bipartite graph). \n", + "Variables represent the unknown random variables in the estimation problem, such as vehicle or landmark positions, sensor calibration parameters, and more.\n", + "Factors represent the algebraic interaction between particular variables using stochastic models, for example wheel odometry between poses, which is depicted with edges. \n", + "A graph of variables and factors is the factorization of the function describing your system, and effectively represents a breakdown of the complex problem describing your robot navigation; \n", + "for example, the graph models the position and orientation (pose) of your robot at any given time relative to landmarks. \n", + "This factorization allows us to solve the optimization (a.k.a. inference) problem for all variables given every measurement described by the factors.\n", + "See Caesar.jl docs [Graph Concepts](https://juliarobotics.org/Caesar.jl/latest/concepts/concepts/#Graph-Concepts) for more detail.\n", + "\n", + "### Let's build a factor graph\n", + "\n", + "First, we will need some packages: \n", + "- `GraphPlot` - for visualizing graphs.\n", + "- `DistributedFactorGraphs` - standardized API for interacting with factor graphs.\n", + "- `IncrementalInference` - Optimization routines for incremental non-parametric and parametric solutions based on factor graphs.\n", + "- `RoME` - Robot Motion Estimate: Tools, Variables, and Factors for SLAM in robotics." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# install packages in case they are not installed\n", + "import Pkg; Pkg.add(\"NavAbilitySDK\"); Pkg.add(\"UUIDs\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "using NavAbilitySDK\n", + "using LinearAlgebra\n", + "using UUIDs" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The first thing to do is setup a client-context to talk with the NavAbility Platform:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# you need a unique userId:robotId, and can keep using that across all tutorials\n", + "userId = \"Guest\"\n", + "robotId = \"ICRA_\"*(string(uuid4())[1:4])\n", + "\n", + "# also create a client connection\n", + "client = NavAbilityHttpsClient()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# You'll need a unique session number each time run a new graph\n", + "sessionId = \"Tutorial1_\"*(string(uuid4())[1:4])\n", + "# context is the object to use below\n", + "context = Client(userId,robotId,sessionId)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Variables and Factors \n", + "\n", + "Variables, denoted as the larger nodes in the figure below, represent state variables of interest such as vehicle or landmark positions, sensor calibration parameters, and more. Variables are likely hidden values that are not directly observed, but we want to estimate them from observed data and at least some minimal algebra structure from probabilistic measurement models.\n", + "\n", + "Factors, the smaller nodes in the figure below, represent the algebraic interaction between particular variables, which is captured through edges. Factors must adhere to the limits of probabilistic models – for example conditional likelihoods capture the likelihood correlations between variables; while priors (unary to one variable) represent absolute information to be introduced. \n", + "\n", + "`NavAbilitySDK.jl` provides variables and factors useful to robotics.\n", + "We start with a `Pose2` variable, i.e. position and orientation in two dimensions.\n", + "Call `addVariable` with a label `x0` and type `Pose2` to add variables to the factor graph" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# lets collect all the async responses and wait at the end\n", + "resultIds = String[]\n", + "# addVariable and keep the transaction ID\n", + "push!(resultIds, \n", + " addVariable(client, context, Variable(\"x0\", :Pose2))\n", + ");" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "We now have a factor graph with one variable, but to solve it we need some additional information. \n", + "In this example, we need the estimated starting point of our robot.\n", + "We use unary factors called priors to represent absolute information to be introduced. \n", + "In this case we use `PriorPose2`, as our variable type is also `Pose2`.\n", + "Since factors represent a probabilistic interaction between variables, we need to specify the distribution our factor will represent. Here we use `FullNormal` which is a [multivariate normal distribution](https://en.wikipedia.org/wiki/Multivariate_normal_distribution). \n", + "\n", + "Let's create a `PriorPose2` unary factor with zero mean and a covariance matrix of (`diagm([0.05,0.05,0.01].^2)`):\n", + "\n", + "$\\Sigma = \\begin{bmatrix} 0.0025 & 0.0 & 0.0 \\\\ 0.0 & 0.0025 & 0.0 \\\\ 0.0 & 0.0 & 0.0001 \\end{bmatrix}$" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "f = Factor(\"x0f1\", \"PriorPose2\", [\"x0\"], \n", + "PriorPose2Data(\n", + " Z=FullNormal(\n", + " [0.0, 0.0, 0.0], \n", + " diagm([0.05, 0.05, 0.01].^2))))\n", + "\n", + "push!(resultIds, addFactor(client, context, f));" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# lets wait to make sure all nodes were added\n", + "waitForCompletion(client, resultIds; expectedStatuses=[\"Complete\"])" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "We can look at the factor graph we have so far using the generated link" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "println(\"https://app.navability.io/cloud/graph/?userId=$userId&robotId=$robotId&sessionId=$sessionId\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The prior is now connected to the variable, `x0`, but it is not initialized yet. Automatic initialization of variables depends on how the factor graph model is constructed. This tutorial demonstrates this behavior by first showing that `x0` is not initialized:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# isInitialized(fg, \"x0\")\n", + "# false" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "**Graph-based Initialization**\n", + "\n", + "Why is `x0` not initialized? Since no other variable nodes have been 'connected to' (or depend) on `x0` and future intentions of the user are unknown, the initialization of `x0` is deferred until the latest possible moment. The NavAbility Platform assumes that the new variables and factors can be initialized when they are solved for the first time.\n", + "\n", + "By delaying initialization of a new variable (say `x0`) until a second newer uninitialized variable (say `x1`) depends on `x0`, the `Caesar.jl` algorithms hope to initialize `x0` with the more information from surrounding variables and factors. Also, note that graph-based initialization of variables is a local operation based only on the neighboring nodes – global inference over the entire graph is shown later in this tutorial." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "**Robot Odometry - Relative Factor**\n", + "\n", + "Next, we want to add an odometry factor that connects our two robot poses `x0` and `x1` together to form a chain.\n", + "Here we use a relative factor of type `Pose2Pose2` with a measurement from pose `x0` to `x1` of (x=1.0,y=0.0,θ=pi/2); the robot drove 1 unit forward (in the x direction).\n", + "Similarly to the prior we added above, we use an `NvNormal` distribution to represent the odometry with mean and covariance:\n", + "\n", + "$\\mu =(x=1, y=0, \\theta=\\frac{\\pi}{2})$\n", + "\n", + "$\\Sigma = \\begin{bmatrix} 0.01 & 0.0 & 0.0 \\\\ 0.0 & 0.01 & 0.0 \\\\ 0.0 & 0.0 & 0.0001 \\end{bmatrix}$\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# add x1\n", + "push!(resultIds, \n", + " addVariable(client, context, Variable(\"x1\", :Pose2))\n", + ")\n", + "\n", + "# add odometry measurement between x0 and x1\n", + "f = Factor(\"x0x1f1\", \"Pose2Pose2\", [\"x0\",\"x1\"], \n", + " Pose2Pose2Data(\n", + " Z=FullNormal(\n", + " [1.0, 0.0, pi/2], \n", + " diagm([0.1, 0.1, 0.01].^2))\n", + " )\n", + ")\n", + "push!(resultIds, addFactor(client, context, f));" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Lets look at the factor graph again" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "println(\"https://app.navability.io/cloud/graph/?userId=$userId&robotId=$robotId&sessionId=$sessionId\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The structure of the graph has now been updated to two variable nodes and two factors." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### Solving\n", + "\n", + "We now have a graph we can solve using the Multi-Modal iSAM (MM-iSAM) algorithm. \n", + "The default solver will perform non-parametric inference/state-estimation over our newly created graph.\n", + "\n", + "Fundamentally, inference is performed via the Bayes (junction) tree where Chapman-Kolmogorov transit integral solutions are based on marginal-joint belief estimation (a sum-product / belief-propagation approximation algorithm). Many benefits such as clique recycling are also available. See the [Solving Graphs](https://juliarobotics.org/Caesar.jl/latest/concepts/solving_graphs/) section in the documentation for more detail." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "push!(resultIds,\n", + " solveSession(client, context)\n", + ");" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### Results\n", + "\n", + "The NavAbility WebApp allows visualization of the belief state over any of the variables." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "println(\"https://app.navability.io/cloud/map/?userId=$userId&robotId=$robotId&sessionId=$sessionId\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "**What is happening**\n", + "\n", + "The figure shows the position and orientation (red forward) for poses `x0` and `x1`. As well as the covariance ellipse. \n", + "Since the solver used was non-parametric, the covariance ellipse is based on a best Gaussian distribution fit of the full belief.\n", + "A few other functions are also handy for interacting with the factor graph, for instance `getVariable` returns the full variable.\n", + "\n", + "\n", + "Or if you are interested in the suggested Parametric Point Estimate (PPE)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "vars = getVariables(client, context; detail=SUMMARY)\n", + "lbls = vars .|> x->x[\"label\"]\n", + "ppes = vars .|> x->float.(x[\"ppes\"][1][\"suggested\"])\n", + "\n", + "println.(lbls .=> ppes);" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "**Parametric point estimates and beliefs**\n", + "\n", + "A PPE can be the maximum or mean of the belief. \n", + "If the belief is a normal distribution, both correspond with its mean. \n", + "However, care should be taken with using PPEs when beliefs might be non-parametric, for example, in a multimodal belief with two peaks, max corresponds with the maximum of the two peaks while the mean will fall somewhere in between them. \n", + "In non-parametric cases, it is better to work with the full belief obtained by the Kernel Density Estimate (KDE). \n", + "Kernel Density Estimation is a non-parametric way to estimate the probability density function of a random variable.\n", + "With the default solver, a full probability density function is always available and can be visualized as shown by the distribution plot feature in the NavAbility WebApp.\n", + "Non-parametric solutions will be discussed in more detail in tutorial 2." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "println(\"https://app.navability.io/cloud/map/?userId=$userId&robotId=$robotId&sessionId=$sessionId\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "More plotting options exist, depending on how you are accessing the data. See the [section on Plotting in the Caesar docs](https://juliarobotics.org/Caesar.jl/latest/concepts/2d_plotting/) for additional detail." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### Adding more poses and a point landmark\n", + "\n", + "So far we worked with the `Pose2` factor type. \n", + "Among others, `NavAbilitySDK` also provides the `Point2` variable and `Pose2Point2BearingRange` factor types, which we will use to represent a landmark sighting in our factor graph.\n", + "We will add a landmark `l1` with bearing range measuremet of bearing=$(\\mu=0,\\sigma=0.03)$ range=$(\\mu=0.5,\\sigma=0.1)$ and continue our robot trajectory by driving around in a square." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# add one landmark\n", + "push!(resultIds,\n", + " addVariable(client, context, Variable(\"l1\", :Point2)))\n", + "\n", + "# add three more poses\n", + "for x in [\"x2\"; \"x3\"; \"x4\"]\n", + " push!(resultIds,\n", + " addVariable(client, context, Variable(x, :Pose2)))\n", + "end\n", + "\n", + "## add Factors\n", + "F = [\n", + " # add landmark observation measurement and\n", + " Factor(\"x0l1f1\", \"Pose2Point2BearingRange\", [\"x0\",\"l1\"], \n", + " Pose2Point2BearingRangeData(\n", + " bearing = Normal(0, 0.03), \n", + " range = Normal(0.5, 0.1))),\n", + " \n", + " # odometry measurements between poses\n", + " Factor(\"x1x2f1\", \"Pose2Pose2\", [\"x1\",\"x2\"], \n", + " Pose2Pose2Data(\n", + " Z=FullNormal(\n", + " [1.0, 0.0, pi/2], \n", + " diagm([0.1, 0.1, 0.01].^2))\n", + " )),\n", + " Factor(\"x2x3f1\", \"Pose2Pose2\", [\"x2\",\"x3\"], \n", + " Pose2Pose2Data(\n", + " Z=FullNormal(\n", + " [1.0, 0.0, pi/2], \n", + " diagm([0.1, 0.1, 0.01].^2))\n", + " )),\n", + " Factor(\"x3x4f1\", \"Pose2Pose2\", [\"x3\",\"x4\"], \n", + " Pose2Pose2Data(\n", + " Z=FullNormal(\n", + " [1.0, 0.0, pi/2], \n", + " diagm([0.1, 0.1, 0.01].^2))\n", + " ))\n", + "]\n", + "\n", + "[push!(resultIds, addFactor(client, context, f)) for f in F];" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Lets go look at the factor graph now, using the WebApp" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "println(\"https://app.navability.io/cloud/graph/?userId=$userId&robotId=$robotId&sessionId=$sessionId\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "We now have a longer odometry chain with one landmark sighting, let's solve the factor graph again so we can have a look at the results." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# lets wait to make sure all the new additions are ready\n", + "waitForCompletion(client, resultIds; expectedStatuses=[\"Complete\"])\n", + "\n", + "# then start a single solve\n", + "push!(resultIds,\n", + " solveSession(client, context)\n", + ");" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The solve will take a bit of time. Just keep watching the geometric visualization, which will automatically update as more of the solution is published" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "println(\"https://app.navability.io/cloud/map/?userId=$userId&robotId=$robotId&sessionId=$sessionId\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "As expected, the robot continued its square trajectory to end off where it started. \n", + "To illustrate a loop closure, we add another bearing range sighting to from pose `x4` to landmark `l1`, solve the graph and plot the new results: " + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# add a loop closure landmark observation\n", + "f = Factor(\"x4l1f1\", \"Pose2Point2BearingRange\", [\"x4\",\"l1\"], \n", + " Pose2Point2BearingRangeData(\n", + " bearing = Normal(0, 0.03), \n", + " range = Normal(0.5, 0.1))\n", + ")\n", + "push!(resultIds, addFactor(client, context, f));\n", + "#\n", + "\n", + "# lets wait to make sure all the new additions are ready\n", + "waitForCompletion(client, resultIds; expectedStatuses=[\"Complete\"])\n", + "\n", + "# then start a single solve\n", + "push!(resultIds,\n", + " solveSession(client, context)\n", + ");" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Lets go look at the final results again, which now includes the loop closure. Use the 'Show Belief', 'Show Distribution' buttons along the bottom for more visual information. You can also trigger new solves (as long as the Global Filter fields are properly set in the far right filter menu).\n", + "\n", + "Use the WebApp hamburger menu on the left to navigate between the graph and geometric map visualization. You can also use the Global Filter menu on the right to set which information to visualize." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "println(\"https://app.navability.io/cloud/map/?userId=$userId&robotId=$robotId&sessionId=$sessionId\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Next Steps\n", + "\n", + "Tutorial 2 will give an introduction to non-parametric solutions.\n", + "\n", + "## Additional Resources\n", + "\n", + "### Variable representation and factor algebraic operations\n", + "\n", + "The variables used in this tutiorail, `Point2` and `Pose2`, are represented as points on manifolds and all algebraic operations, inference/optimization are also performed on manifold.\n", + "For more information on how manifolds are used in Ceaser.jl, refer to the [manifold section in the documentation](https://juliarobotics.org/Caesar.jl/latest/concepts/using_manifolds/).\n", + "\n", + "### Custom Variables and Factors\n", + "\n", + "In most scenarios, the existing variables and factors should be sufficient for most robotics applications. \n", + "Caesar however, is extensible and allows you to easily incorporate your own variable and factor types for specialized applications.\n", + "\n", + "Have a look at the Caesar documentaton if you are interested in creating custom vairables, factors (or priors)\n", + "\n", + "- [Custom variables](https://juliarobotics.org/Caesar.jl/latest/examples/custom_variables/)\n", + "- [Custom prior factors](https://juliarobotics.org/Caesar.jl/latest/examples/basic_definingfactors/)\n", + "- [Custom relative factors](https://juliarobotics.org/Caesar.jl/latest/examples/custom_relative_factors/)\n", + "\n", + "\n", + "### Bayes (Junction) Tree\n", + "\n", + "Inference is performed on the bayes tree see: [Bayes tree principles](https://juliarobotics.org/Caesar.jl/latest/principles/bayestreePrinciples/)" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Julia 1.7.2", + "language": "julia", + "name": "julia-1.7" + }, + "language_info": { + "file_extension": ".jl", + "mimetype": "application/julia", + "name": "julia", + "version": "1.7.2" + }, + "orig_nbformat": 4 + }, + "nbformat": 4, + "nbformat_minor": 2 +} From d2e4416f838dcb766cba86b26054b7d5bbcfdab7 Mon Sep 17 00:00:00 2001 From: dehann Date: Tue, 29 Mar 2022 23:43:01 -0700 Subject: [PATCH 2/4] moved file back to notebook --- julia/navability-sdk/{icra-1-simple.json => icra-1-simple.ipynb} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename julia/navability-sdk/{icra-1-simple.json => icra-1-simple.ipynb} (100%) diff --git a/julia/navability-sdk/icra-1-simple.json b/julia/navability-sdk/icra-1-simple.ipynb similarity index 100% rename from julia/navability-sdk/icra-1-simple.json rename to julia/navability-sdk/icra-1-simple.ipynb From 9d1c1927ccde26335614e81cc23d69076f7c34fd Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Wed, 30 Mar 2022 15:45:26 +0200 Subject: [PATCH 3/4] fix "First, we will need some packages: " section --- Manifest.toml | 20 +- Project.toml | 2 + julia/navability-sdk/icra-1-simple.ipynb | 1148 +++++++++++----------- 3 files changed, 586 insertions(+), 584 deletions(-) diff --git a/Manifest.toml b/Manifest.toml index cc6d0c6..7187b3b 100644 --- a/Manifest.toml +++ b/Manifest.toml @@ -237,9 +237,9 @@ version = "0.18.1" [[Distributions]] deps = ["ChainRulesCore", "DensityInterface", "FillArrays", "LinearAlgebra", "PDMats", "Printf", "QuadGK", "Random", "SparseArrays", "SpecialFunctions", "Statistics", "StatsBase", "StatsFuns", "Test"] -git-tree-sha1 = "c43e992f186abaf9965cc45e372f4693b7754b22" +git-tree-sha1 = "5a4168170ede913a2cd679e53c2123cb4b889795" uuid = "31c24e10-a181-5473-b8eb-7969acd0382f" -version = "0.25.52" +version = "0.25.53" [[DocStringExtensions]] deps = ["LibGit2"] @@ -265,9 +265,9 @@ version = "0.4.1" [[Expat_jll]] deps = ["Artifacts", "JLLWrappers", "Libdl", "Pkg"] -git-tree-sha1 = "ae13fcbc7ab8f16b0856729b050ef0c446aa3492" +git-tree-sha1 = "bad72f730e9e91c08d9427d5e8db95478a3c323d" uuid = "2e619515-83b5-522b-bb60-26c02a35a201" -version = "2.4.4+0" +version = "2.4.8+0" [[ExprTools]] git-tree-sha1 = "56559bbef6ca5ea0c0818fa5c90320398a6fbf8d" @@ -784,10 +784,12 @@ uuid = "356022a1-0364-5f58-8944-0da4b18d706f" version = "0.2.47" [[NavAbilitySDK]] -deps = ["Dates", "Diana", "JSON", "Random"] -git-tree-sha1 = "3d53d2b918ae6802878e6887a5d6be62e495bbee" +deps = ["Dates", "Diana", "DocStringExtensions", "JSON", "LinearAlgebra", "Random"] +git-tree-sha1 = "2093ad30c9ccb60394968486a3ccc7c12e79928d" +repo-rev = "main" +repo-url = "https://github.com/NavAbility/NavAbilitySDK.jl.git" uuid = "f3e6a059-199c-4ada-8143-fcefb97e6165" -version = "0.3.0" +version = "0.4.0" [[Neo4j]] deps = ["Base64", "DataFrames", "DocStringExtensions", "HTTP", "JSON", "Missings"] @@ -979,9 +981,9 @@ version = "0.3.0+0" [[RoME]] deps = ["ApproxManifoldProducts", "CoordinateTransformations", "Dates", "DelimitedFiles", "Distributed", "DistributedFactorGraphs", "Distributions", "DocStringExtensions", "FileIO", "IncrementalInference", "JLD2", "KernelDensityEstimate", "LinearAlgebra", "Manifolds", "ManifoldsBase", "Optim", "ProgressMeter", "Random", "Reexport", "Requires", "Rotations", "StaticArrays", "Statistics", "TensorCast", "TransformUtils"] -git-tree-sha1 = "4203363f034dc947ef6793a943c170ee74ab5754" +git-tree-sha1 = "05a764d9ecbfd66c0a60404ed9a2ba2b9d923ad7" uuid = "91fb55c2-4c03-5a59-ba21-f4ea956187b8" -version = "0.18.1" +version = "0.18.2" [[RoMEPlotting]] deps = ["ApproxManifoldProducts", "Cairo", "Colors", "Compose", "Dates", "DistributedFactorGraphs", "DocStringExtensions", "Fontconfig", "Gadfly", "IncrementalInference", "KernelDensityEstimate", "KernelDensityEstimatePlotting", "LinearAlgebra", "Reexport", "Requires", "RoME", "Statistics", "StatsBase", "TensorCast"] diff --git a/Project.toml b/Project.toml index fbe118b..9306868 100644 --- a/Project.toml +++ b/Project.toml @@ -4,6 +4,8 @@ DistributedFactorGraphs = "b5cc3c7e-6572-11e9-2517-99fb8daf2f04" Fontconfig = "186bb1d3-e1f7-5a2c-a377-96d770f13627" GraphPlot = "a2cc645c-3eea-5389-862e-a155d0052231" IncrementalInference = "904591bb-b899-562f-9e6f-b8df64c7d480" +LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" NavAbilitySDK = "f3e6a059-199c-4ada-8143-fcefb97e6165" RoME = "91fb55c2-4c03-5a59-ba21-f4ea956187b8" RoMEPlotting = "238d586b-a4bf-555c-9891-eda6fc5e55a2" +UUIDs = "cf7118a7-6976-5b1a-9a39-7adc72f591a4" diff --git a/julia/navability-sdk/icra-1-simple.ipynb b/julia/navability-sdk/icra-1-simple.ipynb index ab852d5..e25605d 100644 --- a/julia/navability-sdk/icra-1-simple.ipynb +++ b/julia/navability-sdk/icra-1-simple.ipynb @@ -1,575 +1,573 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# Tutorial 1: Creating and Solving Factor Graphs\n", - "\n", - "## API Version: NavAbilitySDK.jl\n", - "\n", - "In this tutorial you will learn how to create, solve, and explore factor graphs, as well as what variables, factors, and priors are.\n", - "\n", - "## What is a Factor Graph\n", - "\n", - "Wikipedia tells us a factor graph is a bipartite graph representing the factorization of a function.\n", - " \n", - "If you haven't taken some advanced statistics classes you are probably wondering: Bipartite graph? Factorization? I just want to navigate my robot.\n", - "For robotics, factor graphs are the common language used to describe your robot's estimation problem (the function) in a way both humans and computers can understand. \n", - "The estimation problem can be anything from robot localization, structure from motion, calibration to full SLAM with parametric or non-parametric measurements and beliefs.\n", - "A factor graph is a graphical model with two types of nodes, variables and factors, connected by edges between the variables and factors (bipartite graph). \n", - "Variables represent the unknown random variables in the estimation problem, such as vehicle or landmark positions, sensor calibration parameters, and more.\n", - "Factors represent the algebraic interaction between particular variables using stochastic models, for example wheel odometry between poses, which is depicted with edges. \n", - "A graph of variables and factors is the factorization of the function describing your system, and effectively represents a breakdown of the complex problem describing your robot navigation; \n", - "for example, the graph models the position and orientation (pose) of your robot at any given time relative to landmarks. \n", - "This factorization allows us to solve the optimization (a.k.a. inference) problem for all variables given every measurement described by the factors.\n", - "See Caesar.jl docs [Graph Concepts](https://juliarobotics.org/Caesar.jl/latest/concepts/concepts/#Graph-Concepts) for more detail.\n", - "\n", - "### Let's build a factor graph\n", - "\n", - "First, we will need some packages: \n", - "- `GraphPlot` - for visualizing graphs.\n", - "- `DistributedFactorGraphs` - standardized API for interacting with factor graphs.\n", - "- `IncrementalInference` - Optimization routines for incremental non-parametric and parametric solutions based on factor graphs.\n", - "- `RoME` - Robot Motion Estimate: Tools, Variables, and Factors for SLAM in robotics." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# install packages in case they are not installed\n", - "import Pkg; Pkg.add(\"NavAbilitySDK\"); Pkg.add(\"UUIDs\")" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "using NavAbilitySDK\n", - "using LinearAlgebra\n", - "using UUIDs" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The first thing to do is setup a client-context to talk with the NavAbility Platform:" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# you need a unique userId:robotId, and can keep using that across all tutorials\n", - "userId = \"Guest\"\n", - "robotId = \"ICRA_\"*(string(uuid4())[1:4])\n", - "\n", - "# also create a client connection\n", - "client = NavAbilityHttpsClient()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# You'll need a unique session number each time run a new graph\n", - "sessionId = \"Tutorial1_\"*(string(uuid4())[1:4])\n", - "# context is the object to use below\n", - "context = Client(userId,robotId,sessionId)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Variables and Factors \n", - "\n", - "Variables, denoted as the larger nodes in the figure below, represent state variables of interest such as vehicle or landmark positions, sensor calibration parameters, and more. Variables are likely hidden values that are not directly observed, but we want to estimate them from observed data and at least some minimal algebra structure from probabilistic measurement models.\n", - "\n", - "Factors, the smaller nodes in the figure below, represent the algebraic interaction between particular variables, which is captured through edges. Factors must adhere to the limits of probabilistic models – for example conditional likelihoods capture the likelihood correlations between variables; while priors (unary to one variable) represent absolute information to be introduced. \n", - "\n", - "`NavAbilitySDK.jl` provides variables and factors useful to robotics.\n", - "We start with a `Pose2` variable, i.e. position and orientation in two dimensions.\n", - "Call `addVariable` with a label `x0` and type `Pose2` to add variables to the factor graph" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# lets collect all the async responses and wait at the end\n", - "resultIds = String[]\n", - "# addVariable and keep the transaction ID\n", - "push!(resultIds, \n", - " addVariable(client, context, Variable(\"x0\", :Pose2))\n", - ");" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "We now have a factor graph with one variable, but to solve it we need some additional information. \n", - "In this example, we need the estimated starting point of our robot.\n", - "We use unary factors called priors to represent absolute information to be introduced. \n", - "In this case we use `PriorPose2`, as our variable type is also `Pose2`.\n", - "Since factors represent a probabilistic interaction between variables, we need to specify the distribution our factor will represent. Here we use `FullNormal` which is a [multivariate normal distribution](https://en.wikipedia.org/wiki/Multivariate_normal_distribution). \n", - "\n", - "Let's create a `PriorPose2` unary factor with zero mean and a covariance matrix of (`diagm([0.05,0.05,0.01].^2)`):\n", - "\n", - "$\\Sigma = \\begin{bmatrix} 0.0025 & 0.0 & 0.0 \\\\ 0.0 & 0.0025 & 0.0 \\\\ 0.0 & 0.0 & 0.0001 \\end{bmatrix}$" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "f = Factor(\"x0f1\", \"PriorPose2\", [\"x0\"], \n", - "PriorPose2Data(\n", - " Z=FullNormal(\n", - " [0.0, 0.0, 0.0], \n", - " diagm([0.05, 0.05, 0.01].^2))))\n", - "\n", - "push!(resultIds, addFactor(client, context, f));" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# lets wait to make sure all nodes were added\n", - "waitForCompletion(client, resultIds; expectedStatuses=[\"Complete\"])" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "We can look at the factor graph we have so far using the generated link" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "println(\"https://app.navability.io/cloud/graph/?userId=$userId&robotId=$robotId&sessionId=$sessionId\")" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The prior is now connected to the variable, `x0`, but it is not initialized yet. Automatic initialization of variables depends on how the factor graph model is constructed. This tutorial demonstrates this behavior by first showing that `x0` is not initialized:" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# isInitialized(fg, \"x0\")\n", - "# false" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "**Graph-based Initialization**\n", - "\n", - "Why is `x0` not initialized? Since no other variable nodes have been 'connected to' (or depend) on `x0` and future intentions of the user are unknown, the initialization of `x0` is deferred until the latest possible moment. The NavAbility Platform assumes that the new variables and factors can be initialized when they are solved for the first time.\n", - "\n", - "By delaying initialization of a new variable (say `x0`) until a second newer uninitialized variable (say `x1`) depends on `x0`, the `Caesar.jl` algorithms hope to initialize `x0` with the more information from surrounding variables and factors. Also, note that graph-based initialization of variables is a local operation based only on the neighboring nodes – global inference over the entire graph is shown later in this tutorial." - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "**Robot Odometry - Relative Factor**\n", - "\n", - "Next, we want to add an odometry factor that connects our two robot poses `x0` and `x1` together to form a chain.\n", - "Here we use a relative factor of type `Pose2Pose2` with a measurement from pose `x0` to `x1` of (x=1.0,y=0.0,θ=pi/2); the robot drove 1 unit forward (in the x direction).\n", - "Similarly to the prior we added above, we use an `NvNormal` distribution to represent the odometry with mean and covariance:\n", - "\n", - "$\\mu =(x=1, y=0, \\theta=\\frac{\\pi}{2})$\n", - "\n", - "$\\Sigma = \\begin{bmatrix} 0.01 & 0.0 & 0.0 \\\\ 0.0 & 0.01 & 0.0 \\\\ 0.0 & 0.0 & 0.0001 \\end{bmatrix}$\n" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# add x1\n", - "push!(resultIds, \n", - " addVariable(client, context, Variable(\"x1\", :Pose2))\n", - ")\n", - "\n", - "# add odometry measurement between x0 and x1\n", - "f = Factor(\"x0x1f1\", \"Pose2Pose2\", [\"x0\",\"x1\"], \n", - " Pose2Pose2Data(\n", - " Z=FullNormal(\n", - " [1.0, 0.0, pi/2], \n", - " diagm([0.1, 0.1, 0.01].^2))\n", - " )\n", - ")\n", - "push!(resultIds, addFactor(client, context, f));" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Lets look at the factor graph again" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "println(\"https://app.navability.io/cloud/graph/?userId=$userId&robotId=$robotId&sessionId=$sessionId\")" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The structure of the graph has now been updated to two variable nodes and two factors." - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "### Solving\n", - "\n", - "We now have a graph we can solve using the Multi-Modal iSAM (MM-iSAM) algorithm. \n", - "The default solver will perform non-parametric inference/state-estimation over our newly created graph.\n", - "\n", - "Fundamentally, inference is performed via the Bayes (junction) tree where Chapman-Kolmogorov transit integral solutions are based on marginal-joint belief estimation (a sum-product / belief-propagation approximation algorithm). Many benefits such as clique recycling are also available. See the [Solving Graphs](https://juliarobotics.org/Caesar.jl/latest/concepts/solving_graphs/) section in the documentation for more detail." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "push!(resultIds,\n", - " solveSession(client, context)\n", - ");" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "### Results\n", - "\n", - "The NavAbility WebApp allows visualization of the belief state over any of the variables." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "println(\"https://app.navability.io/cloud/map/?userId=$userId&robotId=$robotId&sessionId=$sessionId\")" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "**What is happening**\n", - "\n", - "The figure shows the position and orientation (red forward) for poses `x0` and `x1`. As well as the covariance ellipse. \n", - "Since the solver used was non-parametric, the covariance ellipse is based on a best Gaussian distribution fit of the full belief.\n", - "A few other functions are also handy for interacting with the factor graph, for instance `getVariable` returns the full variable.\n", - "\n", - "\n", - "Or if you are interested in the suggested Parametric Point Estimate (PPE)." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "vars = getVariables(client, context; detail=SUMMARY)\n", - "lbls = vars .|> x->x[\"label\"]\n", - "ppes = vars .|> x->float.(x[\"ppes\"][1][\"suggested\"])\n", - "\n", - "println.(lbls .=> ppes);" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "**Parametric point estimates and beliefs**\n", - "\n", - "A PPE can be the maximum or mean of the belief. \n", - "If the belief is a normal distribution, both correspond with its mean. \n", - "However, care should be taken with using PPEs when beliefs might be non-parametric, for example, in a multimodal belief with two peaks, max corresponds with the maximum of the two peaks while the mean will fall somewhere in between them. \n", - "In non-parametric cases, it is better to work with the full belief obtained by the Kernel Density Estimate (KDE). \n", - "Kernel Density Estimation is a non-parametric way to estimate the probability density function of a random variable.\n", - "With the default solver, a full probability density function is always available and can be visualized as shown by the distribution plot feature in the NavAbility WebApp.\n", - "Non-parametric solutions will be discussed in more detail in tutorial 2." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "println(\"https://app.navability.io/cloud/map/?userId=$userId&robotId=$robotId&sessionId=$sessionId\")" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "More plotting options exist, depending on how you are accessing the data. See the [section on Plotting in the Caesar docs](https://juliarobotics.org/Caesar.jl/latest/concepts/2d_plotting/) for additional detail." - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "### Adding more poses and a point landmark\n", - "\n", - "So far we worked with the `Pose2` factor type. \n", - "Among others, `NavAbilitySDK` also provides the `Point2` variable and `Pose2Point2BearingRange` factor types, which we will use to represent a landmark sighting in our factor graph.\n", - "We will add a landmark `l1` with bearing range measuremet of bearing=$(\\mu=0,\\sigma=0.03)$ range=$(\\mu=0.5,\\sigma=0.1)$ and continue our robot trajectory by driving around in a square." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# add one landmark\n", - "push!(resultIds,\n", - " addVariable(client, context, Variable(\"l1\", :Point2)))\n", - "\n", - "# add three more poses\n", - "for x in [\"x2\"; \"x3\"; \"x4\"]\n", - " push!(resultIds,\n", - " addVariable(client, context, Variable(x, :Pose2)))\n", - "end\n", - "\n", - "## add Factors\n", - "F = [\n", - " # add landmark observation measurement and\n", - " Factor(\"x0l1f1\", \"Pose2Point2BearingRange\", [\"x0\",\"l1\"], \n", - " Pose2Point2BearingRangeData(\n", - " bearing = Normal(0, 0.03), \n", - " range = Normal(0.5, 0.1))),\n", - " \n", - " # odometry measurements between poses\n", - " Factor(\"x1x2f1\", \"Pose2Pose2\", [\"x1\",\"x2\"], \n", - " Pose2Pose2Data(\n", - " Z=FullNormal(\n", - " [1.0, 0.0, pi/2], \n", - " diagm([0.1, 0.1, 0.01].^2))\n", - " )),\n", - " Factor(\"x2x3f1\", \"Pose2Pose2\", [\"x2\",\"x3\"], \n", - " Pose2Pose2Data(\n", - " Z=FullNormal(\n", - " [1.0, 0.0, pi/2], \n", - " diagm([0.1, 0.1, 0.01].^2))\n", - " )),\n", - " Factor(\"x3x4f1\", \"Pose2Pose2\", [\"x3\",\"x4\"], \n", - " Pose2Pose2Data(\n", - " Z=FullNormal(\n", - " [1.0, 0.0, pi/2], \n", - " diagm([0.1, 0.1, 0.01].^2))\n", - " ))\n", - "]\n", - "\n", - "[push!(resultIds, addFactor(client, context, f)) for f in F];" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Lets go look at the factor graph now, using the WebApp" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "println(\"https://app.navability.io/cloud/graph/?userId=$userId&robotId=$robotId&sessionId=$sessionId\")" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "We now have a longer odometry chain with one landmark sighting, let's solve the factor graph again so we can have a look at the results." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# lets wait to make sure all the new additions are ready\n", - "waitForCompletion(client, resultIds; expectedStatuses=[\"Complete\"])\n", - "\n", - "# then start a single solve\n", - "push!(resultIds,\n", - " solveSession(client, context)\n", - ");" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The solve will take a bit of time. Just keep watching the geometric visualization, which will automatically update as more of the solution is published" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "println(\"https://app.navability.io/cloud/map/?userId=$userId&robotId=$robotId&sessionId=$sessionId\")" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "As expected, the robot continued its square trajectory to end off where it started. \n", - "To illustrate a loop closure, we add another bearing range sighting to from pose `x4` to landmark `l1`, solve the graph and plot the new results: " - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# add a loop closure landmark observation\n", - "f = Factor(\"x4l1f1\", \"Pose2Point2BearingRange\", [\"x4\",\"l1\"], \n", - " Pose2Point2BearingRangeData(\n", - " bearing = Normal(0, 0.03), \n", - " range = Normal(0.5, 0.1))\n", - ")\n", - "push!(resultIds, addFactor(client, context, f));\n", - "#\n", - "\n", - "# lets wait to make sure all the new additions are ready\n", - "waitForCompletion(client, resultIds; expectedStatuses=[\"Complete\"])\n", - "\n", - "# then start a single solve\n", - "push!(resultIds,\n", - " solveSession(client, context)\n", - ");" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Lets go look at the final results again, which now includes the loop closure. Use the 'Show Belief', 'Show Distribution' buttons along the bottom for more visual information. You can also trigger new solves (as long as the Global Filter fields are properly set in the far right filter menu).\n", - "\n", - "Use the WebApp hamburger menu on the left to navigate between the graph and geometric map visualization. You can also use the Global Filter menu on the right to set which information to visualize." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "println(\"https://app.navability.io/cloud/map/?userId=$userId&robotId=$robotId&sessionId=$sessionId\")" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Next Steps\n", - "\n", - "Tutorial 2 will give an introduction to non-parametric solutions.\n", - "\n", - "## Additional Resources\n", - "\n", - "### Variable representation and factor algebraic operations\n", - "\n", - "The variables used in this tutiorail, `Point2` and `Pose2`, are represented as points on manifolds and all algebraic operations, inference/optimization are also performed on manifold.\n", - "For more information on how manifolds are used in Ceaser.jl, refer to the [manifold section in the documentation](https://juliarobotics.org/Caesar.jl/latest/concepts/using_manifolds/).\n", - "\n", - "### Custom Variables and Factors\n", - "\n", - "In most scenarios, the existing variables and factors should be sufficient for most robotics applications. \n", - "Caesar however, is extensible and allows you to easily incorporate your own variable and factor types for specialized applications.\n", - "\n", - "Have a look at the Caesar documentaton if you are interested in creating custom vairables, factors (or priors)\n", - "\n", - "- [Custom variables](https://juliarobotics.org/Caesar.jl/latest/examples/custom_variables/)\n", - "- [Custom prior factors](https://juliarobotics.org/Caesar.jl/latest/examples/basic_definingfactors/)\n", - "- [Custom relative factors](https://juliarobotics.org/Caesar.jl/latest/examples/custom_relative_factors/)\n", - "\n", - "\n", - "### Bayes (Junction) Tree\n", - "\n", - "Inference is performed on the bayes tree see: [Bayes tree principles](https://juliarobotics.org/Caesar.jl/latest/principles/bayestreePrinciples/)" - ] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Julia 1.7.2", - "language": "julia", - "name": "julia-1.7" - }, - "language_info": { - "file_extension": ".jl", - "mimetype": "application/julia", - "name": "julia", - "version": "1.7.2" - }, - "orig_nbformat": 4 - }, - "nbformat": 4, - "nbformat_minor": 2 -} +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Tutorial 1: Creating and Solving Factor Graphs\n", + "\n", + "## API Version: NavAbilitySDK.jl\n", + "\n", + "In this tutorial you will learn how to create, solve, and explore factor graphs, as well as what variables, factors, and priors are.\n", + "\n", + "## What is a Factor Graph\n", + "\n", + "Wikipedia tells us a factor graph is a bipartite graph representing the factorization of a function.\n", + " \n", + "If you haven't taken some advanced statistics classes you are probably wondering: Bipartite graph? Factorization? I just want to navigate my robot.\n", + "For robotics, factor graphs are the common language used to describe your robot's estimation problem (the function) in a way both humans and computers can understand. \n", + "The estimation problem can be anything from robot localization, structure from motion, calibration to full SLAM with parametric or non-parametric measurements and beliefs.\n", + "A factor graph is a graphical model with two types of nodes, variables and factors, connected by edges between the variables and factors (bipartite graph). \n", + "Variables represent the unknown random variables in the estimation problem, such as vehicle or landmark positions, sensor calibration parameters, and more.\n", + "Factors represent the algebraic interaction between particular variables using stochastic models, for example wheel odometry between poses, which is depicted with edges. \n", + "A graph of variables and factors is the factorization of the function describing your system, and effectively represents a breakdown of the complex problem describing your robot navigation; \n", + "for example, the graph models the position and orientation (pose) of your robot at any given time relative to landmarks. \n", + "This factorization allows us to solve the optimization (a.k.a. inference) problem for all variables given every measurement described by the factors.\n", + "See Caesar.jl docs [Graph Concepts](https://juliarobotics.org/Caesar.jl/latest/concepts/concepts/#Graph-Concepts) for more detail.\n", + "\n", + "### Let's build a factor graph\n", + "\n", + "First, we will need some packages: \n", + "- `NavAbilitySDK` - Access NavAbility Cloud factor graph features from Julia.\n", + "- `LinearAlgebra` and `UUIDs` - Standard Julia libraries that will be useful." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# install packages in case they are not installed\n", + "import Pkg; Pkg.add(\"NavAbilitySDK\"); Pkg.add(\"UUIDs\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "using NavAbilitySDK\n", + "using LinearAlgebra\n", + "using UUIDs" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The first thing to do is setup a client-context to talk with the NavAbility Platform:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# you need a unique userId:robotId, and can keep using that across all tutorials\n", + "userId = \"Guest\"\n", + "robotId = \"ICRA_\"*(string(uuid4())[1:4])\n", + "\n", + "# also create a client connection\n", + "client = NavAbilityHttpsClient()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# You'll need a unique session number each time you run a new graph\n", + "sessionId = \"Tutorial1_\"*(string(uuid4())[1:4])\n", + "# context is the object to use below\n", + "context = Client(userId,robotId,sessionId)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Variables and Factors \n", + "\n", + "Variables, denoted as the larger nodes in the figure below, represent state variables of interest such as vehicle or landmark positions, sensor calibration parameters, and more. Variables are likely hidden values that are not directly observed, but we want to estimate them from observed data and at least some minimal algebra structure from probabilistic measurement models.\n", + "\n", + "Factors, the smaller nodes in the figure below, represent the algebraic interaction between particular variables, which is captured through edges. Factors must adhere to the limits of probabilistic models – for example conditional likelihoods capture the likelihood correlations between variables; while priors (unary to one variable) represent absolute information to be introduced. \n", + "\n", + "`NavAbilitySDK.jl` provides variables and factors useful to robotics.\n", + "We start with a `Pose2` variable, i.e. position and orientation in two dimensions.\n", + "Call `addVariable` with a label `x0` and type `Pose2` to add variables to the factor graph" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# lets collect all the async responses and wait at the end\n", + "resultIds = String[]\n", + "# addVariable and keep the transaction ID\n", + "push!(resultIds, \n", + " addVariable(client, context, Variable(\"x0\", :Pose2))\n", + ");" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "We now have a factor graph with one variable, but to solve it we need some additional information. \n", + "In this example, we need the estimated starting point of our robot.\n", + "We use unary factors called priors to represent absolute information to be introduced. \n", + "In this case we use `PriorPose2`, as our variable type is also `Pose2`.\n", + "Since factors represent a probabilistic interaction between variables, we need to specify the distribution our factor will represent. Here we use `FullNormal` which is a [multivariate normal distribution](https://en.wikipedia.org/wiki/Multivariate_normal_distribution). \n", + "\n", + "Let's create a `PriorPose2` unary factor with zero mean and a covariance matrix of (`diagm([0.05,0.05,0.01].^2)`):\n", + "\n", + "$\\Sigma = \\begin{bmatrix} 0.0025 & 0.0 & 0.0 \\\\ 0.0 & 0.0025 & 0.0 \\\\ 0.0 & 0.0 & 0.0001 \\end{bmatrix}$" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "f = Factor(\"x0f1\", \"PriorPose2\", [\"x0\"], \n", + "PriorPose2Data(\n", + " Z=FullNormal(\n", + " [0.0, 0.0, 0.0], \n", + " diagm([0.05, 0.05, 0.01].^2))))\n", + "\n", + "push!(resultIds, addFactor(client, context, f));" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# lets wait to make sure all nodes were added\n", + "waitForCompletion(client, resultIds; expectedStatuses=[\"Complete\"])" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "We can look at the factor graph we have so far using the generated link" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "println(\"https://app.navability.io/cloud/graph/?userId=$userId&robotId=$robotId&sessionId=$sessionId\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The prior is now connected to the variable, `x0`, but it is not initialized yet. Automatic initialization of variables depends on how the factor graph model is constructed. This tutorial demonstrates this behavior by first showing that `x0` is not initialized:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# isInitialized(fg, \"x0\")\n", + "# false" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "**Graph-based Initialization**\n", + "\n", + "Why is `x0` not initialized? Since no other variable nodes have been 'connected to' (or depend) on `x0` and future intentions of the user are unknown, the initialization of `x0` is deferred until the latest possible moment. The NavAbility Platform assumes that the new variables and factors can be initialized when they are solved for the first time.\n", + "\n", + "By delaying initialization of a new variable (say `x0`) until a second newer uninitialized variable (say `x1`) depends on `x0`, the `Caesar.jl` algorithms hope to initialize `x0` with the more information from surrounding variables and factors. Also, note that graph-based initialization of variables is a local operation based only on the neighboring nodes – global inference over the entire graph is shown later in this tutorial." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "**Robot Odometry - Relative Factor**\n", + "\n", + "Next, we want to add an odometry factor that connects our two robot poses `x0` and `x1` together to form a chain.\n", + "Here we use a relative factor of type `Pose2Pose2` with a measurement from pose `x0` to `x1` of (x=1.0,y=0.0,θ=pi/2); the robot drove 1 unit forward (in the x direction).\n", + "Similarly to the prior we added above, we use an `NvNormal` distribution to represent the odometry with mean and covariance:\n", + "\n", + "$\\mu =(x=1, y=0, \\theta=\\frac{\\pi}{2})$\n", + "\n", + "$\\Sigma = \\begin{bmatrix} 0.01 & 0.0 & 0.0 \\\\ 0.0 & 0.01 & 0.0 \\\\ 0.0 & 0.0 & 0.0001 \\end{bmatrix}$\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# add x1\n", + "push!(resultIds, \n", + " addVariable(client, context, Variable(\"x1\", :Pose2))\n", + ")\n", + "\n", + "# add odometry measurement between x0 and x1\n", + "f = Factor(\"x0x1f1\", \"Pose2Pose2\", [\"x0\",\"x1\"], \n", + " Pose2Pose2Data(\n", + " Z=FullNormal(\n", + " [1.0, 0.0, pi/2], \n", + " diagm([0.1, 0.1, 0.01].^2))\n", + " )\n", + ")\n", + "push!(resultIds, addFactor(client, context, f));" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Lets look at the factor graph again" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "println(\"https://app.navability.io/cloud/graph/?userId=$userId&robotId=$robotId&sessionId=$sessionId\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The structure of the graph has now been updated to two variable nodes and two factors." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### Solving\n", + "\n", + "We now have a graph we can solve using the Multi-Modal iSAM (MM-iSAM) algorithm. \n", + "The default solver will perform non-parametric inference/state-estimation over our newly created graph.\n", + "\n", + "Fundamentally, inference is performed via the Bayes (junction) tree where Chapman-Kolmogorov transit integral solutions are based on marginal-joint belief estimation (a sum-product / belief-propagation approximation algorithm). Many benefits such as clique recycling are also available. See the [Solving Graphs](https://juliarobotics.org/Caesar.jl/latest/concepts/solving_graphs/) section in the documentation for more detail." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "push!(resultIds,\n", + " solveSession(client, context)\n", + ");" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### Results\n", + "\n", + "The NavAbility WebApp allows visualization of the belief state over any of the variables." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "println(\"https://app.navability.io/cloud/map/?userId=$userId&robotId=$robotId&sessionId=$sessionId\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "**What is happening**\n", + "\n", + "The figure shows the position and orientation (red forward) for poses `x0` and `x1`. As well as the covariance ellipse. \n", + "Since the solver used was non-parametric, the covariance ellipse is based on a best Gaussian distribution fit of the full belief.\n", + "A few other functions are also handy for interacting with the factor graph, for instance `getVariable` returns the full variable.\n", + "\n", + "\n", + "Or if you are interested in the suggested Parametric Point Estimate (PPE)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "vars = getVariables(client, context; detail=SUMMARY)\n", + "lbls = vars .|> x->x[\"label\"]\n", + "ppes = vars .|> x->float.(x[\"ppes\"][1][\"suggested\"])\n", + "\n", + "println.(lbls .=> ppes);" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "**Parametric point estimates and beliefs**\n", + "\n", + "A PPE can be the maximum or mean of the belief. \n", + "If the belief is a normal distribution, both correspond with its mean. \n", + "However, care should be taken with using PPEs when beliefs might be non-parametric, for example, in a multimodal belief with two peaks, max corresponds with the maximum of the two peaks while the mean will fall somewhere in between them. \n", + "In non-parametric cases, it is better to work with the full belief obtained by the Kernel Density Estimate (KDE). \n", + "Kernel Density Estimation is a non-parametric way to estimate the probability density function of a random variable.\n", + "With the default solver, a full probability density function is always available and can be visualized as shown by the distribution plot feature in the NavAbility WebApp.\n", + "Non-parametric solutions will be discussed in more detail in tutorial 2." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "println(\"https://app.navability.io/cloud/map/?userId=$userId&robotId=$robotId&sessionId=$sessionId\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "More plotting options exist, depending on how you are accessing the data. See the [section on Plotting in the Caesar docs](https://juliarobotics.org/Caesar.jl/latest/concepts/2d_plotting/) for additional detail." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### Adding more poses and a point landmark\n", + "\n", + "So far we worked with the `Pose2` factor type. \n", + "Among others, `NavAbilitySDK` also provides the `Point2` variable and `Pose2Point2BearingRange` factor types, which we will use to represent a landmark sighting in our factor graph.\n", + "We will add a landmark `l1` with bearing range measuremet of bearing=$(\\mu=0,\\sigma=0.03)$ range=$(\\mu=0.5,\\sigma=0.1)$ and continue our robot trajectory by driving around in a square." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# add one landmark\n", + "push!(resultIds,\n", + " addVariable(client, context, Variable(\"l1\", :Point2)))\n", + "\n", + "# add three more poses\n", + "for x in [\"x2\"; \"x3\"; \"x4\"]\n", + " push!(resultIds,\n", + " addVariable(client, context, Variable(x, :Pose2)))\n", + "end\n", + "\n", + "## add Factors\n", + "F = [\n", + " # add landmark observation measurement and\n", + " Factor(\"x0l1f1\", \"Pose2Point2BearingRange\", [\"x0\",\"l1\"], \n", + " Pose2Point2BearingRangeData(\n", + " bearing = Normal(0, 0.03), \n", + " range = Normal(0.5, 0.1))),\n", + " \n", + " # odometry measurements between poses\n", + " Factor(\"x1x2f1\", \"Pose2Pose2\", [\"x1\",\"x2\"], \n", + " Pose2Pose2Data(\n", + " Z=FullNormal(\n", + " [1.0, 0.0, pi/2], \n", + " diagm([0.1, 0.1, 0.01].^2))\n", + " )),\n", + " Factor(\"x2x3f1\", \"Pose2Pose2\", [\"x2\",\"x3\"], \n", + " Pose2Pose2Data(\n", + " Z=FullNormal(\n", + " [1.0, 0.0, pi/2], \n", + " diagm([0.1, 0.1, 0.01].^2))\n", + " )),\n", + " Factor(\"x3x4f1\", \"Pose2Pose2\", [\"x3\",\"x4\"], \n", + " Pose2Pose2Data(\n", + " Z=FullNormal(\n", + " [1.0, 0.0, pi/2], \n", + " diagm([0.1, 0.1, 0.01].^2))\n", + " ))\n", + "]\n", + "\n", + "[push!(resultIds, addFactor(client, context, f)) for f in F];" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Let's go look at the factor graph now, using the WebApp" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "println(\"https://app.navability.io/cloud/graph/?userId=$userId&robotId=$robotId&sessionId=$sessionId\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "We now have a longer odometry chain with one landmark sighting, let's solve the factor graph again so we can have a look at the results." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# lets wait to make sure all the new additions are ready\n", + "waitForCompletion(client, resultIds; expectedStatuses=[\"Complete\"])\n", + "\n", + "# then start a single solve\n", + "push!(resultIds,\n", + " solveSession(client, context)\n", + ");" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The solve will take a bit of time. Just keep watching the geometric visualization, which will automatically update as more of the solution is published" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "println(\"https://app.navability.io/cloud/map/?userId=$userId&robotId=$robotId&sessionId=$sessionId\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "As expected, the robot continued its square trajectory to end off where it started. \n", + "To illustrate a loop closure, we add another bearing range sighting to from pose `x4` to landmark `l1`, solve the graph and plot the new results: " + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# add a loop closure landmark observation\n", + "f = Factor(\"x4l1f1\", \"Pose2Point2BearingRange\", [\"x4\",\"l1\"], \n", + " Pose2Point2BearingRangeData(\n", + " bearing = Normal(0, 0.03), \n", + " range = Normal(0.5, 0.1))\n", + ")\n", + "push!(resultIds, addFactor(client, context, f));\n", + "#\n", + "\n", + "# lets wait to make sure all the new additions are ready\n", + "waitForCompletion(client, resultIds; expectedStatuses=[\"Complete\"])\n", + "\n", + "# then start a single solve\n", + "push!(resultIds,\n", + " solveSession(client, context)\n", + ");" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Let's go look at the final results again, which now includes the loop closure. Use the 'Show Belief', 'Show Distribution' buttons along the bottom for more visual information. You can also trigger new solves (as long as the Global Filter fields are properly set in the far right filter menu).\n", + "\n", + "Use the WebApp hamburger menu on the left to navigate between the graph and geometric map visualization. You can also use the Global Filter menu on the right to set which information to visualize." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "println(\"https://app.navability.io/cloud/map/?userId=$userId&robotId=$robotId&sessionId=$sessionId\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Next Steps\n", + "\n", + "Tutorial 2 will give an introduction to non-parametric solutions.\n", + "\n", + "## Additional Resources\n", + "\n", + "### Variable representation and factor algebraic operations\n", + "\n", + "The variables used in this tutiorail, `Point2` and `Pose2`, are represented as points on manifolds and all algebraic operations, inference/optimization are also performed on manifold.\n", + "For more information on how manifolds are used in Ceaser.jl, refer to the [manifold section in the documentation](https://juliarobotics.org/Caesar.jl/latest/concepts/using_manifolds/).\n", + "\n", + "### Custom Variables and Factors\n", + "\n", + "In most scenarios, the existing variables and factors should be sufficient for most robotics applications. \n", + "Caesar however, is extensible and allows you to easily incorporate your own variable and factor types for specialized applications.\n", + "\n", + "Have a look at the Caesar documentaton if you are interested in creating custom vairables, factors (or priors)\n", + "\n", + "- [Custom variables](https://juliarobotics.org/Caesar.jl/latest/examples/custom_variables/)\n", + "- [Custom prior factors](https://juliarobotics.org/Caesar.jl/latest/examples/basic_definingfactors/)\n", + "- [Custom relative factors](https://juliarobotics.org/Caesar.jl/latest/examples/custom_relative_factors/)\n", + "\n", + "\n", + "### Bayes (Junction) Tree\n", + "\n", + "Inference is performed on the bayes tree see: [Bayes tree principles](https://juliarobotics.org/Caesar.jl/latest/principles/bayestreePrinciples/)" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Julia 1.7.1", + "language": "julia", + "name": "julia-1.7" + }, + "language_info": { + "file_extension": ".jl", + "mimetype": "application/julia", + "name": "julia", + "version": "1.7.1" + }, + "orig_nbformat": 4 + }, + "nbformat": 4, + "nbformat_minor": 2 +} From 84884c25d4527aa65d2934f7cc13b3057c7113fc Mon Sep 17 00:00:00 2001 From: dehann Date: Wed, 30 Mar 2022 22:16:40 -0700 Subject: [PATCH 4/4] fixes --- julia/navability-sdk/icra-1-simple.ipynb | 1142 +++++++++++----------- 1 file changed, 567 insertions(+), 575 deletions(-) diff --git a/julia/navability-sdk/icra-1-simple.ipynb b/julia/navability-sdk/icra-1-simple.ipynb index ab852d5..139770d 100644 --- a/julia/navability-sdk/icra-1-simple.ipynb +++ b/julia/navability-sdk/icra-1-simple.ipynb @@ -1,575 +1,567 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# Tutorial 1: Creating and Solving Factor Graphs\n", - "\n", - "## API Version: NavAbilitySDK.jl\n", - "\n", - "In this tutorial you will learn how to create, solve, and explore factor graphs, as well as what variables, factors, and priors are.\n", - "\n", - "## What is a Factor Graph\n", - "\n", - "Wikipedia tells us a factor graph is a bipartite graph representing the factorization of a function.\n", - " \n", - "If you haven't taken some advanced statistics classes you are probably wondering: Bipartite graph? Factorization? I just want to navigate my robot.\n", - "For robotics, factor graphs are the common language used to describe your robot's estimation problem (the function) in a way both humans and computers can understand. \n", - "The estimation problem can be anything from robot localization, structure from motion, calibration to full SLAM with parametric or non-parametric measurements and beliefs.\n", - "A factor graph is a graphical model with two types of nodes, variables and factors, connected by edges between the variables and factors (bipartite graph). \n", - "Variables represent the unknown random variables in the estimation problem, such as vehicle or landmark positions, sensor calibration parameters, and more.\n", - "Factors represent the algebraic interaction between particular variables using stochastic models, for example wheel odometry between poses, which is depicted with edges. \n", - "A graph of variables and factors is the factorization of the function describing your system, and effectively represents a breakdown of the complex problem describing your robot navigation; \n", - "for example, the graph models the position and orientation (pose) of your robot at any given time relative to landmarks. \n", - "This factorization allows us to solve the optimization (a.k.a. inference) problem for all variables given every measurement described by the factors.\n", - "See Caesar.jl docs [Graph Concepts](https://juliarobotics.org/Caesar.jl/latest/concepts/concepts/#Graph-Concepts) for more detail.\n", - "\n", - "### Let's build a factor graph\n", - "\n", - "First, we will need some packages: \n", - "- `GraphPlot` - for visualizing graphs.\n", - "- `DistributedFactorGraphs` - standardized API for interacting with factor graphs.\n", - "- `IncrementalInference` - Optimization routines for incremental non-parametric and parametric solutions based on factor graphs.\n", - "- `RoME` - Robot Motion Estimate: Tools, Variables, and Factors for SLAM in robotics." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# install packages in case they are not installed\n", - "import Pkg; Pkg.add(\"NavAbilitySDK\"); Pkg.add(\"UUIDs\")" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "using NavAbilitySDK\n", - "using LinearAlgebra\n", - "using UUIDs" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The first thing to do is setup a client-context to talk with the NavAbility Platform:" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# you need a unique userId:robotId, and can keep using that across all tutorials\n", - "userId = \"Guest\"\n", - "robotId = \"ICRA_\"*(string(uuid4())[1:4])\n", - "\n", - "# also create a client connection\n", - "client = NavAbilityHttpsClient()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# You'll need a unique session number each time run a new graph\n", - "sessionId = \"Tutorial1_\"*(string(uuid4())[1:4])\n", - "# context is the object to use below\n", - "context = Client(userId,robotId,sessionId)" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Variables and Factors \n", - "\n", - "Variables, denoted as the larger nodes in the figure below, represent state variables of interest such as vehicle or landmark positions, sensor calibration parameters, and more. Variables are likely hidden values that are not directly observed, but we want to estimate them from observed data and at least some minimal algebra structure from probabilistic measurement models.\n", - "\n", - "Factors, the smaller nodes in the figure below, represent the algebraic interaction between particular variables, which is captured through edges. Factors must adhere to the limits of probabilistic models – for example conditional likelihoods capture the likelihood correlations between variables; while priors (unary to one variable) represent absolute information to be introduced. \n", - "\n", - "`NavAbilitySDK.jl` provides variables and factors useful to robotics.\n", - "We start with a `Pose2` variable, i.e. position and orientation in two dimensions.\n", - "Call `addVariable` with a label `x0` and type `Pose2` to add variables to the factor graph" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# lets collect all the async responses and wait at the end\n", - "resultIds = String[]\n", - "# addVariable and keep the transaction ID\n", - "push!(resultIds, \n", - " addVariable(client, context, Variable(\"x0\", :Pose2))\n", - ");" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "We now have a factor graph with one variable, but to solve it we need some additional information. \n", - "In this example, we need the estimated starting point of our robot.\n", - "We use unary factors called priors to represent absolute information to be introduced. \n", - "In this case we use `PriorPose2`, as our variable type is also `Pose2`.\n", - "Since factors represent a probabilistic interaction between variables, we need to specify the distribution our factor will represent. Here we use `FullNormal` which is a [multivariate normal distribution](https://en.wikipedia.org/wiki/Multivariate_normal_distribution). \n", - "\n", - "Let's create a `PriorPose2` unary factor with zero mean and a covariance matrix of (`diagm([0.05,0.05,0.01].^2)`):\n", - "\n", - "$\\Sigma = \\begin{bmatrix} 0.0025 & 0.0 & 0.0 \\\\ 0.0 & 0.0025 & 0.0 \\\\ 0.0 & 0.0 & 0.0001 \\end{bmatrix}$" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "f = Factor(\"x0f1\", \"PriorPose2\", [\"x0\"], \n", - "PriorPose2Data(\n", - " Z=FullNormal(\n", - " [0.0, 0.0, 0.0], \n", - " diagm([0.05, 0.05, 0.01].^2))))\n", - "\n", - "push!(resultIds, addFactor(client, context, f));" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# lets wait to make sure all nodes were added\n", - "waitForCompletion(client, resultIds; expectedStatuses=[\"Complete\"])" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "We can look at the factor graph we have so far using the generated link" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "println(\"https://app.navability.io/cloud/graph/?userId=$userId&robotId=$robotId&sessionId=$sessionId\")" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The prior is now connected to the variable, `x0`, but it is not initialized yet. Automatic initialization of variables depends on how the factor graph model is constructed. This tutorial demonstrates this behavior by first showing that `x0` is not initialized:" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# isInitialized(fg, \"x0\")\n", - "# false" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "**Graph-based Initialization**\n", - "\n", - "Why is `x0` not initialized? Since no other variable nodes have been 'connected to' (or depend) on `x0` and future intentions of the user are unknown, the initialization of `x0` is deferred until the latest possible moment. The NavAbility Platform assumes that the new variables and factors can be initialized when they are solved for the first time.\n", - "\n", - "By delaying initialization of a new variable (say `x0`) until a second newer uninitialized variable (say `x1`) depends on `x0`, the `Caesar.jl` algorithms hope to initialize `x0` with the more information from surrounding variables and factors. Also, note that graph-based initialization of variables is a local operation based only on the neighboring nodes – global inference over the entire graph is shown later in this tutorial." - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "**Robot Odometry - Relative Factor**\n", - "\n", - "Next, we want to add an odometry factor that connects our two robot poses `x0` and `x1` together to form a chain.\n", - "Here we use a relative factor of type `Pose2Pose2` with a measurement from pose `x0` to `x1` of (x=1.0,y=0.0,θ=pi/2); the robot drove 1 unit forward (in the x direction).\n", - "Similarly to the prior we added above, we use an `NvNormal` distribution to represent the odometry with mean and covariance:\n", - "\n", - "$\\mu =(x=1, y=0, \\theta=\\frac{\\pi}{2})$\n", - "\n", - "$\\Sigma = \\begin{bmatrix} 0.01 & 0.0 & 0.0 \\\\ 0.0 & 0.01 & 0.0 \\\\ 0.0 & 0.0 & 0.0001 \\end{bmatrix}$\n" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# add x1\n", - "push!(resultIds, \n", - " addVariable(client, context, Variable(\"x1\", :Pose2))\n", - ")\n", - "\n", - "# add odometry measurement between x0 and x1\n", - "f = Factor(\"x0x1f1\", \"Pose2Pose2\", [\"x0\",\"x1\"], \n", - " Pose2Pose2Data(\n", - " Z=FullNormal(\n", - " [1.0, 0.0, pi/2], \n", - " diagm([0.1, 0.1, 0.01].^2))\n", - " )\n", - ")\n", - "push!(resultIds, addFactor(client, context, f));" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Lets look at the factor graph again" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "println(\"https://app.navability.io/cloud/graph/?userId=$userId&robotId=$robotId&sessionId=$sessionId\")" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The structure of the graph has now been updated to two variable nodes and two factors." - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "### Solving\n", - "\n", - "We now have a graph we can solve using the Multi-Modal iSAM (MM-iSAM) algorithm. \n", - "The default solver will perform non-parametric inference/state-estimation over our newly created graph.\n", - "\n", - "Fundamentally, inference is performed via the Bayes (junction) tree where Chapman-Kolmogorov transit integral solutions are based on marginal-joint belief estimation (a sum-product / belief-propagation approximation algorithm). Many benefits such as clique recycling are also available. See the [Solving Graphs](https://juliarobotics.org/Caesar.jl/latest/concepts/solving_graphs/) section in the documentation for more detail." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "push!(resultIds,\n", - " solveSession(client, context)\n", - ");" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "### Results\n", - "\n", - "The NavAbility WebApp allows visualization of the belief state over any of the variables." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "println(\"https://app.navability.io/cloud/map/?userId=$userId&robotId=$robotId&sessionId=$sessionId\")" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "**What is happening**\n", - "\n", - "The figure shows the position and orientation (red forward) for poses `x0` and `x1`. As well as the covariance ellipse. \n", - "Since the solver used was non-parametric, the covariance ellipse is based on a best Gaussian distribution fit of the full belief.\n", - "A few other functions are also handy for interacting with the factor graph, for instance `getVariable` returns the full variable.\n", - "\n", - "\n", - "Or if you are interested in the suggested Parametric Point Estimate (PPE)." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "vars = getVariables(client, context; detail=SUMMARY)\n", - "lbls = vars .|> x->x[\"label\"]\n", - "ppes = vars .|> x->float.(x[\"ppes\"][1][\"suggested\"])\n", - "\n", - "println.(lbls .=> ppes);" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "**Parametric point estimates and beliefs**\n", - "\n", - "A PPE can be the maximum or mean of the belief. \n", - "If the belief is a normal distribution, both correspond with its mean. \n", - "However, care should be taken with using PPEs when beliefs might be non-parametric, for example, in a multimodal belief with two peaks, max corresponds with the maximum of the two peaks while the mean will fall somewhere in between them. \n", - "In non-parametric cases, it is better to work with the full belief obtained by the Kernel Density Estimate (KDE). \n", - "Kernel Density Estimation is a non-parametric way to estimate the probability density function of a random variable.\n", - "With the default solver, a full probability density function is always available and can be visualized as shown by the distribution plot feature in the NavAbility WebApp.\n", - "Non-parametric solutions will be discussed in more detail in tutorial 2." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "println(\"https://app.navability.io/cloud/map/?userId=$userId&robotId=$robotId&sessionId=$sessionId\")" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "More plotting options exist, depending on how you are accessing the data. See the [section on Plotting in the Caesar docs](https://juliarobotics.org/Caesar.jl/latest/concepts/2d_plotting/) for additional detail." - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "### Adding more poses and a point landmark\n", - "\n", - "So far we worked with the `Pose2` factor type. \n", - "Among others, `NavAbilitySDK` also provides the `Point2` variable and `Pose2Point2BearingRange` factor types, which we will use to represent a landmark sighting in our factor graph.\n", - "We will add a landmark `l1` with bearing range measuremet of bearing=$(\\mu=0,\\sigma=0.03)$ range=$(\\mu=0.5,\\sigma=0.1)$ and continue our robot trajectory by driving around in a square." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# add one landmark\n", - "push!(resultIds,\n", - " addVariable(client, context, Variable(\"l1\", :Point2)))\n", - "\n", - "# add three more poses\n", - "for x in [\"x2\"; \"x3\"; \"x4\"]\n", - " push!(resultIds,\n", - " addVariable(client, context, Variable(x, :Pose2)))\n", - "end\n", - "\n", - "## add Factors\n", - "F = [\n", - " # add landmark observation measurement and\n", - " Factor(\"x0l1f1\", \"Pose2Point2BearingRange\", [\"x0\",\"l1\"], \n", - " Pose2Point2BearingRangeData(\n", - " bearing = Normal(0, 0.03), \n", - " range = Normal(0.5, 0.1))),\n", - " \n", - " # odometry measurements between poses\n", - " Factor(\"x1x2f1\", \"Pose2Pose2\", [\"x1\",\"x2\"], \n", - " Pose2Pose2Data(\n", - " Z=FullNormal(\n", - " [1.0, 0.0, pi/2], \n", - " diagm([0.1, 0.1, 0.01].^2))\n", - " )),\n", - " Factor(\"x2x3f1\", \"Pose2Pose2\", [\"x2\",\"x3\"], \n", - " Pose2Pose2Data(\n", - " Z=FullNormal(\n", - " [1.0, 0.0, pi/2], \n", - " diagm([0.1, 0.1, 0.01].^2))\n", - " )),\n", - " Factor(\"x3x4f1\", \"Pose2Pose2\", [\"x3\",\"x4\"], \n", - " Pose2Pose2Data(\n", - " Z=FullNormal(\n", - " [1.0, 0.0, pi/2], \n", - " diagm([0.1, 0.1, 0.01].^2))\n", - " ))\n", - "]\n", - "\n", - "[push!(resultIds, addFactor(client, context, f)) for f in F];" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Lets go look at the factor graph now, using the WebApp" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "println(\"https://app.navability.io/cloud/graph/?userId=$userId&robotId=$robotId&sessionId=$sessionId\")" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "We now have a longer odometry chain with one landmark sighting, let's solve the factor graph again so we can have a look at the results." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# lets wait to make sure all the new additions are ready\n", - "waitForCompletion(client, resultIds; expectedStatuses=[\"Complete\"])\n", - "\n", - "# then start a single solve\n", - "push!(resultIds,\n", - " solveSession(client, context)\n", - ");" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The solve will take a bit of time. Just keep watching the geometric visualization, which will automatically update as more of the solution is published" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "println(\"https://app.navability.io/cloud/map/?userId=$userId&robotId=$robotId&sessionId=$sessionId\")" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "As expected, the robot continued its square trajectory to end off where it started. \n", - "To illustrate a loop closure, we add another bearing range sighting to from pose `x4` to landmark `l1`, solve the graph and plot the new results: " - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "# add a loop closure landmark observation\n", - "f = Factor(\"x4l1f1\", \"Pose2Point2BearingRange\", [\"x4\",\"l1\"], \n", - " Pose2Point2BearingRangeData(\n", - " bearing = Normal(0, 0.03), \n", - " range = Normal(0.5, 0.1))\n", - ")\n", - "push!(resultIds, addFactor(client, context, f));\n", - "#\n", - "\n", - "# lets wait to make sure all the new additions are ready\n", - "waitForCompletion(client, resultIds; expectedStatuses=[\"Complete\"])\n", - "\n", - "# then start a single solve\n", - "push!(resultIds,\n", - " solveSession(client, context)\n", - ");" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "Lets go look at the final results again, which now includes the loop closure. Use the 'Show Belief', 'Show Distribution' buttons along the bottom for more visual information. You can also trigger new solves (as long as the Global Filter fields are properly set in the far right filter menu).\n", - "\n", - "Use the WebApp hamburger menu on the left to navigate between the graph and geometric map visualization. You can also use the Global Filter menu on the right to set which information to visualize." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "println(\"https://app.navability.io/cloud/map/?userId=$userId&robotId=$robotId&sessionId=$sessionId\")" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Next Steps\n", - "\n", - "Tutorial 2 will give an introduction to non-parametric solutions.\n", - "\n", - "## Additional Resources\n", - "\n", - "### Variable representation and factor algebraic operations\n", - "\n", - "The variables used in this tutiorail, `Point2` and `Pose2`, are represented as points on manifolds and all algebraic operations, inference/optimization are also performed on manifold.\n", - "For more information on how manifolds are used in Ceaser.jl, refer to the [manifold section in the documentation](https://juliarobotics.org/Caesar.jl/latest/concepts/using_manifolds/).\n", - "\n", - "### Custom Variables and Factors\n", - "\n", - "In most scenarios, the existing variables and factors should be sufficient for most robotics applications. \n", - "Caesar however, is extensible and allows you to easily incorporate your own variable and factor types for specialized applications.\n", - "\n", - "Have a look at the Caesar documentaton if you are interested in creating custom vairables, factors (or priors)\n", - "\n", - "- [Custom variables](https://juliarobotics.org/Caesar.jl/latest/examples/custom_variables/)\n", - "- [Custom prior factors](https://juliarobotics.org/Caesar.jl/latest/examples/basic_definingfactors/)\n", - "- [Custom relative factors](https://juliarobotics.org/Caesar.jl/latest/examples/custom_relative_factors/)\n", - "\n", - "\n", - "### Bayes (Junction) Tree\n", - "\n", - "Inference is performed on the bayes tree see: [Bayes tree principles](https://juliarobotics.org/Caesar.jl/latest/principles/bayestreePrinciples/)" - ] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Julia 1.7.2", - "language": "julia", - "name": "julia-1.7" - }, - "language_info": { - "file_extension": ".jl", - "mimetype": "application/julia", - "name": "julia", - "version": "1.7.2" - }, - "orig_nbformat": 4 - }, - "nbformat": 4, - "nbformat_minor": 2 -} +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Tutorial 1: Creating and Solving Factor Graphs\n", + "\n", + "## API Version: NavAbilitySDK.jl\n", + "\n", + "In this tutorial you will learn how to create, solve, and explore factor graphs, as well as what variables, factors, and priors are.\n", + "\n", + "## What is a Factor Graph\n", + "\n", + "Wikipedia tells us a factor graph is a bipartite graph representing the factorization of a function.\n", + " \n", + "If you haven't taken some advanced statistics classes you are probably wondering: Bipartite graph? Factorization? I just want to navigate my robot.\n", + "For robotics, factor graphs are the common language used to describe your robot's estimation problem (the function) in a way both humans and computers can understand. \n", + "The estimation problem can be anything from robot localization, structure from motion, calibration to full SLAM with parametric or non-parametric measurements and beliefs.\n", + "A factor graph is a graphical model with two types of nodes, variables and factors, connected by edges between the variables and factors (bipartite graph). \n", + "Variables represent the unknown random variables in the estimation problem, such as vehicle or landmark positions, sensor calibration parameters, and more.\n", + "Factors represent the algebraic interaction between particular variables using stochastic models, for example wheel odometry between poses, which is depicted with edges. \n", + "A graph of variables and factors is the factorization of the function describing your system, and effectively represents a breakdown of the complex problem describing your robot navigation; \n", + "for example, the graph models the position and orientation (pose) of your robot at any given time relative to landmarks. \n", + "This factorization allows us to solve the optimization (a.k.a. inference) problem for all variables given every measurement described by the factors.\n", + "See Caesar.jl docs [Graph Concepts](https://juliarobotics.org/Caesar.jl/latest/concepts/concepts/#Graph-Concepts) for more detail.\n", + "\n", + "### Let's build a factor graph\n", + "\n", + "First, we will need some packages: \n", + "- `GraphPlot` - for visualizing graphs.\n", + "- `DistributedFactorGraphs` - standardized API for interacting with factor graphs.\n", + "- `IncrementalInference` - Optimization routines for incremental non-parametric and parametric solutions based on factor graphs.\n", + "- `RoME` - Robot Motion Estimate: Tools, Variables, and Factors for SLAM in robotics." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# install packages in case they are not installed\n", + "import Pkg; Pkg.add(\"NavAbilitySDK\"); Pkg.add(\"UUIDs\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "using NavAbilitySDK\n", + "using LinearAlgebra\n", + "using UUIDs" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The first thing to do is setup a client-context to talk with the NavAbility Platform:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# you need a unique userId:robotId, and can keep using that across all tutorials\n", + "userId = \"Guest\"\n", + "robotId = \"ICRA_\"*(string(uuid4())[1:4])\n", + "\n", + "# also create a client connection\n", + "client = NavAbilityHttpsClient()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# You'll need a unique session number each time run a new graph\n", + "sessionId = \"Tutorial1_\"*(string(uuid4())[1:4])\n", + "# context is the object to use below\n", + "context = Client(userId,robotId,sessionId)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Variables and Factors \n", + "\n", + "Variables, denoted as the larger nodes in the figure below, represent state variables of interest such as vehicle or landmark positions, sensor calibration parameters, and more. Variables are likely hidden values that are not directly observed, but we want to estimate them from observed data and at least some minimal algebra structure from probabilistic measurement models.\n", + "\n", + "Factors, the smaller nodes in the figure below, represent the algebraic interaction between particular variables, which is captured through edges. Factors must adhere to the limits of probabilistic models – for example conditional likelihoods capture the likelihood correlations between variables; while priors (unary to one variable) represent absolute information to be introduced. \n", + "\n", + "`NavAbilitySDK.jl` provides variables and factors useful to robotics.\n", + "We start with a `Pose2` variable, i.e. position and orientation in two dimensions.\n", + "Call `addVariable` with a label `x0` and type `Pose2` to add variables to the factor graph" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# lets collect all the async responses and wait at the end\n", + "resultIds = String[]\n", + "# addVariable and keep the transaction ID\n", + "push!(resultIds, \n", + " addVariable(client, context, Variable(\"x0\", :Pose2))\n", + ");" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "We now have a factor graph with one variable, but to solve it we need some additional information. \n", + "In this example, we need the estimated starting point of our robot.\n", + "We use unary factors called priors to represent absolute information to be introduced. \n", + "In this case we use `PriorPose2`, as our variable type is also `Pose2`.\n", + "Since factors represent a probabilistic interaction between variables, we need to specify the distribution our factor will represent. Here we use `FullNormal` which is a [multivariate normal distribution](https://en.wikipedia.org/wiki/Multivariate_normal_distribution). \n", + "\n", + "Let's create a `PriorPose2` unary factor with zero mean and a covariance matrix of (`diagm([0.05,0.05,0.01].^2)`):\n", + "\n", + "$\\Sigma = \\begin{bmatrix} 0.0025 & 0.0 & 0.0 \\\\ 0.0 & 0.0025 & 0.0 \\\\ 0.0 & 0.0 & 0.0001 \\end{bmatrix}$" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "f = Factor(\"x0f1\", \"PriorPose2\", [\"x0\"], \n", + "PriorPose2Data(\n", + " Z=FullNormal(\n", + " [0.0, 0.0, 0.0], \n", + " diagm([0.05, 0.05, 0.01].^2))))\n", + "\n", + "push!(resultIds, addFactor(client, context, f));" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# lets wait to make sure all nodes were added\n", + "waitForCompletion(client, resultIds; expectedStatuses=[\"Complete\"])" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "We can look at the factor graph we have so far using the generated link" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "println(\"https://app.navability.io/cloud/graph/?userId=$userId&robotStartsWith=$robotId&sessionStartsWith=$sessionId\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The prior is now connected to the variable, `x0`, but it is not initialized yet. Automatic initialization of variables depends on how the factor graph model is constructed. So far, `x0` has not been initialized." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "**Graph-based Initialization**\n", + "\n", + "Why is `x0` not initialized? Since no other variable nodes have been 'connected to' (or depend) on `x0` and future intentions of the user are unknown, the initialization of `x0` is deferred until the latest possible moment. The NavAbility Platform assumes that the new variables and factors can be initialized when they are solved for the first time.\n", + "\n", + "By delaying initialization of a new variable (say `x0`) until a second newer uninitialized variable (say `x1`) depends on `x0`, the `Caesar.jl` algorithms wait to initialize `x0` with the more information from surrounding variables and factors. Also, note that graph-based initialization of variables is a local operation based only on the neighboring nodes – global inference over the entire graph is shown later in this tutorial." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "**Robot Odometry - Relative Factor**\n", + "\n", + "Next, we want to add an odometry factor that connects our two robot poses `x0` and `x1` together to form a chain.\n", + "Here we use a relative factor of type `Pose2Pose2` with a measurement from pose `x0` to `x1` of (x=1.0,y=0.0,θ=pi/2); the robot drove 1 unit forward (in the x direction).\n", + "Similarly to the prior we added above, we use an `FullNormal` distribution to represent the odometry with mean and covariance:\n", + "\n", + "$\\mu =(x=1, y=0, \\theta=\\frac{\\pi}{2})$\n", + "\n", + "$\\Sigma = \\begin{bmatrix} 0.01 & 0.0 & 0.0 \\\\ 0.0 & 0.01 & 0.0 \\\\ 0.0 & 0.0 & 0.0001 \\end{bmatrix}$\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# add x1\n", + "push!(resultIds, \n", + " addVariable(client, context, Variable(\"x1\", :Pose2))\n", + ")\n", + "\n", + "# add odometry measurement between x0 and x1\n", + "f = Factor(\"x0x1f1\", \"Pose2Pose2\", [\"x0\",\"x1\"], \n", + " Pose2Pose2Data(\n", + " Z=FullNormal(\n", + " [1.0, 0.0, pi/2], \n", + " diagm([0.1, 0.1, 0.01].^2))\n", + " )\n", + ")\n", + "push!(resultIds, addFactor(client, context, f));" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Lets look at the factor graph again" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "println(\"https://app.navability.io/cloud/graph/?userId=$userId&robotStartsWith=$robotId&sessionStartsWith=$sessionId\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The structure of the graph has now been updated to two variable nodes and two factors." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### Solving\n", + "\n", + "We now have a graph we can solve using the Multi-Modal iSAM (MM-iSAM) algorithm. \n", + "The default solver will perform non-parametric inference/state-estimation over our newly created graph.\n", + "\n", + "Fundamentally, inference is performed via the Bayes (junction) tree where Chapman-Kolmogorov transit integral solutions are based on marginal-joint belief estimation (a sum-product / belief-propagation approximation algorithm). Many benefits such as clique recycling are also available. See the [Solving Graphs](https://juliarobotics.org/Caesar.jl/latest/concepts/solving_graphs/) section in the documentation for more detail." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "push!(resultIds,\n", + " solveSession(client, context)\n", + ");" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### Results\n", + "\n", + "The NavAbility WebApp allows visualization of the belief state over any of the variables. Also try with Belief and Distribution buttons to see more of the underlying posterior marginal belief estimates." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "println(\"https://app.navability.io/cloud/map/?userStartsWith=$userId&robotStartsWith=$robotId&sessionStartsWith=$sessionId\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "**What is happening**\n", + "\n", + "The figure shows the position and orientation (red forward) for poses `x0` and `x1`. As well as the covariance ellipse. \n", + "Since the solver used was non-parametric, the covariance ellipse is based on a best Gaussian distribution fit of the full belief.\n", + "A few other functions are also handy for interacting with the factor graph, for instance `getVariable` returns the full variable.\n", + "\n", + "\n", + "Or if you are interested in the suggested Parametric Point Estimate (PPE)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "vars = getVariables(client, context; detail=SUMMARY)\n", + "lbls = vars .|> x->x[\"label\"]\n", + "ppes = vars .|> x->float.(x[\"ppes\"][1][\"suggested\"])\n", + "\n", + "println.(lbls .=> ppes);" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "**Parametric point estimates and beliefs**\n", + "\n", + "A PPE can be the maximum or mean of the belief. \n", + "If the belief is a normal distribution, both correspond with its mean. \n", + "However, care should be taken with using PPEs when beliefs might be non-parametric, for example, in a multimodal belief with two peaks, max corresponds with the maximum of the two peaks while the mean will fall somewhere in between them. \n", + "In non-parametric cases, it is better to work with the full belief obtained by the Kernel Density Estimate (KDE). \n", + "Kernel Density Estimation is a non-parametric way to estimate the probability density function of a random variable.\n", + "With the default solver, a full probability density function is always available and can be visualized as shown by the distribution plot feature in the NavAbility WebApp.\n", + "Non-parametric solutions will be discussed in more detail in tutorial 2." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "println(\"https://app.navability.io/cloud/map/?userId=$userId&robotStartsWith=$robotId&sessionStartsWith=$sessionId\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "More plotting options exist, depending on how you are accessing the data. See the [section on Plotting in the Caesar docs](https://juliarobotics.org/Caesar.jl/latest/concepts/2d_plotting/) for additional detail." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### Adding more poses and a point landmark\n", + "\n", + "So far we worked with the `Pose2` factor type. \n", + "Among others, `NavAbilitySDK` also provides the `Point2` variable and `Pose2Point2BearingRange` factor types, which we will use to represent a landmark sighting in our factor graph.\n", + "We will add a landmark `l1` with bearing range measuremet of bearing=$(\\mu=0,\\sigma=0.03)$ range=$(\\mu=0.5,\\sigma=0.1)$ and continue our robot trajectory by driving around in a square." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# add one landmark\n", + "push!(resultIds,\n", + " addVariable(client, context, Variable(\"l1\", :Point2)))\n", + "\n", + "# add three more poses\n", + "for x in [\"x2\"; \"x3\"; \"x4\"]\n", + " push!(resultIds,\n", + " addVariable(client, context, Variable(x, :Pose2)))\n", + "end\n", + "\n", + "## add Factors\n", + "F = [\n", + " # add landmark observation measurement and\n", + " Factor(\"x0l1f1\", \"Pose2Point2BearingRange\", [\"x0\",\"l1\"], \n", + " Pose2Point2BearingRangeData(\n", + " bearing = Normal(0, 0.03), \n", + " range = Normal(0.5, 0.1))),\n", + " \n", + " # odometry measurements between poses\n", + " Factor(\"x1x2f1\", \"Pose2Pose2\", [\"x1\",\"x2\"], \n", + " Pose2Pose2Data(\n", + " Z=FullNormal(\n", + " [1.0, 0.0, pi/2], \n", + " diagm([0.1, 0.1, 0.01].^2))\n", + " )),\n", + " Factor(\"x2x3f1\", \"Pose2Pose2\", [\"x2\",\"x3\"], \n", + " Pose2Pose2Data(\n", + " Z=FullNormal(\n", + " [1.0, 0.0, pi/2], \n", + " diagm([0.1, 0.1, 0.01].^2))\n", + " )),\n", + " Factor(\"x3x4f1\", \"Pose2Pose2\", [\"x3\",\"x4\"], \n", + " Pose2Pose2Data(\n", + " Z=FullNormal(\n", + " [1.0, 0.0, pi/2], \n", + " diagm([0.1, 0.1, 0.01].^2))\n", + " ))\n", + "]\n", + "\n", + "[push!(resultIds, addFactor(client, context, f)) for f in F];" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Lets go look at the factor graph now, using the WebApp" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "println(\"https://app.navability.io/cloud/graph/?userId=$userId&robotStartsWith=$robotId&sessionStartsWith=$sessionId\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "We now have a longer odometry chain with one landmark sighting, let's solve the factor graph again so we can have a look at the results." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# lets wait to make sure all the new additions are ready\n", + "waitForCompletion(client, resultIds; expectedStatuses=[\"Complete\"])\n", + "\n", + "# then start a single solve\n", + "println(\"Start a new solve...\")\n", + "push!(resultIds,\n", + " solveSession(client, context)\n", + ");" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The solve will take a bit of time. Just keep watching the geometric visualization, which will automatically update as more of the solution is published" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "println(\"https://app.navability.io/cloud/map/?userId=$userId&robotStartsWith=$robotId&sessionStartsWith=$sessionId\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "As expected, the robot continued its square trajectory to end off where it started. \n", + "To illustrate a loop closure, we add another bearing range sighting to from pose `x4` to landmark `l1`, solve the graph and plot the new results: " + ] + }, + { + "cell_type": "code", + "execution_count": 22, + "metadata": {}, + "outputs": [], + "source": [ + "# add a loop closure landmark observation\n", + "f = Factor(\"x4l1f1\", \"Pose2Point2BearingRange\", [\"x4\",\"l1\"], \n", + " Pose2Point2BearingRangeData(\n", + " bearing = Normal(0, 0.03), \n", + " range = Normal(0.5, 0.1))\n", + ")\n", + "push!(resultIds, addFactor(client, context, f));\n", + "#\n", + "\n", + "# lets wait to make sure all the new additions are ready\n", + "waitForCompletion(client, resultIds; expectedStatuses=[\"Complete\"])\n", + "\n", + "# then start a single solve\n", + "println(\"Start new solve...\")\n", + "push!(resultIds,\n", + " solveSession(client, context)\n", + ");" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Lets go look at the final results again, which now includes the loop closure. Use the 'Show Belief', 'Show Distribution' buttons along the bottom for more visual information. You can also trigger new solves (as long as the Global Filter fields are properly set in the far right filter menu).\n", + "\n", + "Use the WebApp hamburger menu on the left to navigate between the graph and geometric map visualization. You can also use the Global Filter menu on the right to set which information to visualize." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "println(\"https://app.navability.io/cloud/map/?userId=$userId&robotStartsWith=$robotId&sessionStartsWith=$sessionId\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Next Steps\n", + "\n", + "Tutorial 2 will give an introduction to non-parametric solutions.\n", + "\n", + "## Additional Resources\n", + "\n", + "### Variable representation and factor algebraic operations\n", + "\n", + "The variables used in this tutiorail, `Point2` and `Pose2`, are represented as points on manifolds and all algebraic operations, inference/optimization are also performed on manifold.\n", + "For more information on how manifolds are used in Ceaser.jl, refer to the [manifold section in the documentation](https://juliarobotics.org/Caesar.jl/latest/concepts/using_manifolds/).\n", + "\n", + "### Custom Variables and Factors\n", + "\n", + "In most scenarios, the existing variables and factors should be sufficient for most robotics applications. \n", + "Caesar however, is extensible and allows you to easily incorporate your own variable and factor types for specialized applications.\n", + "\n", + "Have a look at the Caesar documentaton if you are interested in creating custom vairables, factors (or priors)\n", + "\n", + "- [Custom variables](https://juliarobotics.org/Caesar.jl/latest/examples/custom_variables/)\n", + "- [Custom prior factors](https://juliarobotics.org/Caesar.jl/latest/examples/basic_definingfactors/)\n", + "- [Custom relative factors](https://juliarobotics.org/Caesar.jl/latest/examples/custom_relative_factors/)\n", + "\n", + "\n", + "### Bayes (Junction) Tree\n", + "\n", + "Inference is performed on the bayes tree see: [Bayes tree principles](https://juliarobotics.org/Caesar.jl/latest/principles/bayestreePrinciples/)" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Julia 1.7.2", + "language": "julia", + "name": "julia-1.7" + }, + "language_info": { + "file_extension": ".jl", + "mimetype": "application/julia", + "name": "julia", + "version": "1.7.2" + }, + "orig_nbformat": 4 + }, + "nbformat": 4, + "nbformat_minor": 2 +}