Added examples to site
Frank Dellaert
parent
ed6038d6f9
commit
3e8a29ae13
|
|
@ -0,0 +1,3 @@
|
|||
# Examples
|
||||
|
||||
This section contains python examples in interactive Python notebooks (`*.ipynb`). Python notebooks with an <img src="https://colab.research.google.com/assets/colab-badge.svg" alt="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.
|
||||
|
|
@ -1075,9 +1075,14 @@ class PinholeCamera {
|
|||
pair<gtsam::Point2, bool> projectSafe(const gtsam::Point3& pw) const;
|
||||
gtsam::Point2 project(const gtsam::Point3& point);
|
||||
gtsam::Point2 project(const gtsam::Point3& point,
|
||||
Eigen::Ref<Eigen::MatrixXd> Dpose,
|
||||
Eigen::Ref<Eigen::MatrixXd> Dpoint,
|
||||
Eigen::Ref<Eigen::MatrixXd> Dcal);
|
||||
Eigen::Ref<Eigen::MatrixXd> Dpose);
|
||||
gtsam::Point2 project(const gtsam::Point3& point,
|
||||
Eigen::Ref<Eigen::MatrixXd> Dpose,
|
||||
Eigen::Ref<Eigen::MatrixXd> Dpoint);
|
||||
gtsam::Point2 project(const gtsam::Point3& point,
|
||||
Eigen::Ref<Eigen::MatrixXd> Dpose,
|
||||
Eigen::Ref<Eigen::MatrixXd> Dpoint,
|
||||
Eigen::Ref<Eigen::MatrixXd> Dcal);
|
||||
gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
|
||||
gtsam::Point3 backproject(const gtsam::Point2& p, double depth,
|
||||
Eigen::Ref<Eigen::MatrixXd> Dresult_dpose,
|
||||
|
|
|
|||
5
myst.yml
5
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
|
||||
|
|
|
|||
|
|
@ -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": [
|
||||
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/python/examples/CameraResectioning.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
|
||||
]
|
||||
},
|
||||
{
|
||||
"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
|
||||
}
|
||||
|
|
@ -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()
|
||||
|
|
@ -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 *"
|
||||
]
|
||||
},
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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": [
|
||||
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/python/gtsam/examples/RangeISAMExample_plaza2.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {
|
||||
"tags": [
|
||||
"remove-cell"
|
||||
],
|
||||
"vscode": {
|
||||
"languageId": "markdown"
|
||||
}
|
||||
},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"%pip install --quiet gtsam-develop"
|
||||
]
|
||||
},
|
||||
{
|
||||
|
|
|
|||
Loading…
Reference in New Issue