diff --git a/doc/examples.md b/doc/examples.md new file mode 100644 index 000000000..14300a03c --- /dev/null +++ b/doc/examples.md @@ -0,0 +1,3 @@ +# Examples + +This section contains python examples in interactive Python notebooks (`*.ipynb`). Python notebooks with an Open In Colab button near the top can be opened in your browser, where you can run the files yourself and make edits to play with and understand GTSAM. \ No newline at end of file diff --git a/gtsam/user_guide.md b/doc/user_guide.md similarity index 100% rename from gtsam/user_guide.md rename to doc/user_guide.md diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index e01394bfe..8c6e70ef4 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -1075,9 +1075,14 @@ class PinholeCamera { pair projectSafe(const gtsam::Point3& pw) const; gtsam::Point2 project(const gtsam::Point3& point); gtsam::Point2 project(const gtsam::Point3& point, - Eigen::Ref Dpose, - Eigen::Ref Dpoint, - Eigen::Ref Dcal); + Eigen::Ref Dpose); + gtsam::Point2 project(const gtsam::Point3& point, + Eigen::Ref Dpose, + Eigen::Ref Dpoint); + gtsam::Point2 project(const gtsam::Point3& point, + Eigen::Ref Dpose, + Eigen::Ref Dpoint, + Eigen::Ref Dcal); gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; gtsam::Point3 backproject(const gtsam::Point2& p, double depth, Eigen::Ref Dresult_dpose, diff --git a/myst.yml b/myst.yml index 7984b2d20..21df32d84 100644 --- a/myst.yml +++ b/myst.yml @@ -8,7 +8,7 @@ project: toc: - file: README.md - file: INSTALL.md - - file: ./gtsam/user_guide.md + - file: ./doc/user_guide.md children: - file: ./gtsam/geometry/geometry.md children: @@ -16,6 +16,9 @@ project: - file: ./gtsam/nonlinear/nonlinear.md children: - pattern: ./gtsam/nonlinear/doc/* + - file: ./doc/examples.md + children: + - pattern: ./python/gtsam/examples/*.ipynb site: nav: - title: Getting started diff --git a/python/gtsam/examples/CameraResectioning.ipynb b/python/gtsam/examples/CameraResectioning.ipynb new file mode 100644 index 000000000..e037dbfff --- /dev/null +++ b/python/gtsam/examples/CameraResectioning.ipynb @@ -0,0 +1,188 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Camera Resectioning Example\n", + "\n", + "This is a 1:1 transcription of CameraResectioning.cpp, but using custom factors." + ] + }, + { + "cell_type": "markdown", + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "source": [ + "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\n", + "Atlanta, Georgia 30332-0415\n", + "All Rights Reserved\n", + "\n", + "Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n", + "\n", + "See LICENSE for the license information" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "tags": [ + "remove-cell" + ], + "vscode": { + "languageId": "markdown" + } + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam-develop" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "from gtsam import Cal3_S2, CustomFactor, LevenbergMarquardtOptimizer, KeyVector\n", + "from gtsam import NonlinearFactor, NonlinearFactorGraph\n", + "from gtsam import PinholeCameraCal3_S2, Point2, Point3, Pose3, Rot3, Values\n", + "from gtsam.noiseModel import Base as SharedNoiseModel, Diagonal\n", + "from gtsam.symbol_shorthand import X" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "def resectioning_factor(\n", + " model: SharedNoiseModel,\n", + " key: int,\n", + " calib: Cal3_S2,\n", + " p: Point2,\n", + " P: Point3,\n", + ") -> NonlinearFactor:\n", + "\n", + " def error_func(this: CustomFactor, v: Values, H: list[np.ndarray]) -> np.ndarray:\n", + " pose = v.atPose3(this.keys()[0])\n", + " camera = PinholeCameraCal3_S2(pose, calib)\n", + " if H is None:\n", + " return camera.project(P) - p\n", + " Dpose = np.zeros((2, 6), order=\"F\")\n", + " result = camera.project(P, Dpose) - p\n", + " H[0] = Dpose\n", + " return result\n", + "\n", + " return CustomFactor(model, KeyVector([key]), error_func)\n" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Assumptions:\n", + "- Camera: $f = 1$, Image: $100\\times100$, center: $50, 50.0$\n", + "- Pose (ground truth): $(X_w, -Y_w, -Z_w, [0,0,2.0]^T)$\n", + "- Known landmarks: $(10,10,0), (-10,10,0), (-10,-10,0), (10,-10,0)$\n", + "- Perfect measurements: $(55,45), (45,45), (45,55), (55,55)$\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Final result:\n", + "\n", + "Values with 1 values:\n", + "Value x1: (gtsam::Pose3)\n", + "R: [\n", + "\t1, 0, 0;\n", + "\t0, -1, 0;\n", + "\t0, 0, -1\n", + "]\n", + "t: 0 0 2\n", + "\n" + ] + } + ], + "source": [ + "# Create camera intrinsic parameters\n", + "calibration = Cal3_S2(1, 1, 0, 50, 50)\n", + "\n", + "# 1. create graph\n", + "graph = NonlinearFactorGraph()\n", + "\n", + "# 2. add factors to the graph\n", + "measurement_noise = Diagonal.Sigmas(np.array([0.5, 0.5]))\n", + "graph.add(\n", + " resectioning_factor(\n", + " measurement_noise, X(1), calibration, Point2(55, 45), Point3(10, 10, 0)\n", + " )\n", + ")\n", + "graph.add(\n", + " resectioning_factor(\n", + " measurement_noise, X(1), calibration, Point2(45, 45), Point3(-10, 10, 0)\n", + " )\n", + ")\n", + "graph.add(\n", + " resectioning_factor(\n", + " measurement_noise, X(1), calibration, Point2(45, 55), Point3(-10, -10, 0)\n", + " )\n", + ")\n", + "graph.add(\n", + " resectioning_factor(\n", + " measurement_noise, X(1), calibration, Point2(55, 55), Point3(10, -10, 0)\n", + " )\n", + ")\n", + "\n", + "# 3. Create an initial estimate for the camera pose\n", + "initial: Values = Values()\n", + "initial.insert(X(1), Pose3(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(0, 0, 1)))\n", + "\n", + "# 4. Optimize the graph using Levenberg-Marquardt\n", + "result: Values = LevenbergMarquardtOptimizer(graph, initial).optimize()\n", + "result.print(\"Final result:\\n\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/python/gtsam/examples/CameraResectioning.py b/python/gtsam/examples/CameraResectioning.py deleted file mode 100644 index e962b40bb..000000000 --- a/python/gtsam/examples/CameraResectioning.py +++ /dev/null @@ -1,85 +0,0 @@ -# pylint: disable=consider-using-from-import,invalid-name,no-name-in-module,no-member,missing-function-docstring -""" -This is a 1:1 transcription of CameraResectioning.cpp. -""" -import numpy as np -from gtsam import Cal3_S2, CustomFactor, LevenbergMarquardtOptimizer, KeyVector -from gtsam import NonlinearFactor, NonlinearFactorGraph -from gtsam import PinholeCameraCal3_S2, Point2, Point3, Pose3, Rot3, Values -from gtsam.noiseModel import Base as SharedNoiseModel, Diagonal -from gtsam.symbol_shorthand import X - - -def resectioning_factor( - model: SharedNoiseModel, - key: int, - calib: Cal3_S2, - p: Point2, - P: Point3, -) -> NonlinearFactor: - - def error_func(this: CustomFactor, v: Values, H: list[np.ndarray]) -> np.ndarray: - pose = v.atPose3(this.keys()[0]) - camera = PinholeCameraCal3_S2(pose, calib) - if H is None: - return camera.project(P) - p - Dpose = np.zeros((2, 6), order="F") - Dpoint = np.zeros((2, 3), order="F") - Dcal = np.zeros((2, 5), order="F") - result = camera.project(P, Dpose, Dpoint, Dcal) - p - H[0] = Dpose - return result - - return CustomFactor(model, KeyVector([key]), error_func) - - -def main() -> None: - """ - Camera: f = 1, Image: 100x100, center: 50, 50.0 - Pose (ground truth): (Xw, -Yw, -Zw, [0,0,2.0]') - Known landmarks: - 3D Points: (10,10,0) (-10,10,0) (-10,-10,0) (10,-10,0) - Perfect measurements: - 2D Point: (55,45) (45,45) (45,55) (55,55) - """ - - # read camera intrinsic parameters - calib = Cal3_S2(1, 1, 0, 50, 50) - - # 1. create graph - graph = NonlinearFactorGraph() - - # 2. add factors to the graph - measurement_noise = Diagonal.Sigmas(np.array([0.5, 0.5])) - graph.add( - resectioning_factor( - measurement_noise, X(1), calib, Point2(55, 45), Point3(10, 10, 0) - ) - ) - graph.add( - resectioning_factor( - measurement_noise, X(1), calib, Point2(45, 45), Point3(-10, 10, 0) - ) - ) - graph.add( - resectioning_factor( - measurement_noise, X(1), calib, Point2(45, 55), Point3(-10, -10, 0) - ) - ) - graph.add( - resectioning_factor( - measurement_noise, X(1), calib, Point2(55, 55), Point3(10, -10, 0) - ) - ) - - # 3. Create an initial estimate for the camera pose - initial: Values = Values() - initial.insert(X(1), Pose3(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(0, 0, 1))) - - # 4. Optimize the graph using Levenberg-Marquardt - result: Values = LevenbergMarquardtOptimizer(graph, initial).optimize() - result.print("Final result:\n") - - -if __name__ == "__main__": - main() diff --git a/python/gtsam/examples/EqF.ipynb b/python/gtsam/examples/EqF.ipynb index 33d7f74da..59ee2c525 100644 --- a/python/gtsam/examples/EqF.ipynb +++ b/python/gtsam/examples/EqF.ipynb @@ -8,16 +8,36 @@ "\n", "Implementing the example in [Fornasier et al, 2022, Overcoming Bias: Equivariant Filter Design for Biased Attitude Estimation with Online Calibration](https://arxiv.org/pdf/2209.12038).\n", "\n", - "This notebook uses Alessandro Fornasier's equivariant filter code (https://github.com/aau-cns/ABC-EqF) converted to use GTSAM's libraries.\n", - "Authors: Jennifer Oum & Darshan Rajasekaran\n", + "This notebook uses [Alessandro Fornasier's equivariant filter code](https://github.com/aau-cns/ABC-EqF) converted to use GTSAM's libraries.\n", "\n", - "We start by installing gtsam (GTSAM's python wrapper) and gtbook." + "Authors: Jennifer Oum & Darshan Rajasekaran" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "source": [ + "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\n", + "Atlanta, Georgia 30332-0415\n", + "All Rights Reserved\n", + "\n", + "Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n", + "\n", + "See LICENSE for the license information" ] }, { "cell_type": "code", - "execution_count": 68, - "metadata": {}, + "execution_count": null, + "metadata": { + "tags": [ + "remove-cell" + ] + }, "outputs": [ { "name": "stdout", @@ -28,12 +48,13 @@ } ], "source": [ + "# We start by installing gtsam (GTSAM's python wrapper) and gtbook.\n", "%pip install --quiet gtsam gtbook" ] }, { "cell_type": "code", - "execution_count": 69, + "execution_count": null, "metadata": {}, "outputs": [], "source": [ @@ -47,7 +68,7 @@ "import gtsam\n", "from gtsam import findExampleDataFile, Rot3\n", "\n", - "from EqF import *\n" + "from EqF import *" ] }, { diff --git a/python/gtsam/examples/RangeISAMExample_plaza2.ipynb b/python/gtsam/examples/RangeISAMExample_plaza2.ipynb index f11636606..d8b5ff4eb 100644 --- a/python/gtsam/examples/RangeISAMExample_plaza2.ipynb +++ b/python/gtsam/examples/RangeISAMExample_plaza2.ipynb @@ -3,6 +3,21 @@ { "cell_type": "markdown", "metadata": {}, + "source": [ + "# Range SLAM with iSAM\n", + "\n", + "A 2D Range SLAM example, with iSAM and smart range factors\n", + "\n", + "Author: Frank Dellaert" + ] + }, + { + "cell_type": "markdown", + "metadata": { + "tags": [ + "remove-cell" + ] + }, "source": [ "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\n", "Atlanta, Georgia 30332-0415\n", @@ -10,11 +25,30 @@ "\n", "Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n", "\n", - "See LICENSE for the license information\n", - "\n", - "A 2D Range SLAM example, with iSAM and smart range factors\n", - "\n", - "Author: Frank Dellaert" + "See LICENSE for the license information" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "tags": [ + "remove-cell" + ], + "vscode": { + "languageId": "markdown" + } + }, + "outputs": [], + "source": [ + "%pip install --quiet gtsam-develop" ] }, {