223 lines
7.8 KiB
Plaintext
223 lines
7.8 KiB
Plaintext
{
|
|
"cells": [
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "intro_md"
|
|
},
|
|
"source": [
|
|
"# OrientedPlane3 Factors"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "desc_md"
|
|
},
|
|
"source": [
|
|
"This header defines factors for working with planar landmarks represented by `gtsam.OrientedPlane3`.\n",
|
|
"`OrientedPlane3` represents a plane using normalized coefficients $(n_x, n_y, n_z, d)$, where $(n_x, n_y, n_z)$ is the unit normal vector and $d$ is the distance from the origin along the normal.\n",
|
|
"\n",
|
|
"Factors defined:\n",
|
|
"* `OrientedPlane3Factor`: A binary factor connecting a `Pose3` and an `OrientedPlane3`. It measures the difference between the plane parameters as observed from the given pose and a measured plane.\n",
|
|
"* `OrientedPlane3DirectionPrior`: A unary factor on an `OrientedPlane3`. It penalizes the difference between the plane's normal direction and a measured direction (represented by the normal of a measured `OrientedPlane3`). **Note:** The factor error is 3D, but only constrains 2 degrees of freedom (direction). Consider using a more specific direction factor if only direction is measured."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "colab_badge_md"
|
|
},
|
|
"source": [
|
|
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/OrientedPlane3Factor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 1,
|
|
"metadata": {
|
|
"id": "pip_code",
|
|
"tags": [
|
|
"remove-cell"
|
|
]
|
|
},
|
|
"outputs": [],
|
|
"source": [
|
|
"try:\n",
|
|
" import google.colab\n",
|
|
" %pip install --quiet gtsam-develop\n",
|
|
"except ImportError:\n",
|
|
" pass # Not running on Colab, do nothing"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 2,
|
|
"metadata": {
|
|
"id": "imports_code"
|
|
},
|
|
"outputs": [],
|
|
"source": [
|
|
"import gtsam\n",
|
|
"import numpy as np\n",
|
|
"from gtsam import Pose3, OrientedPlane3, Point3, Rot3, Values\n",
|
|
"from gtsam import OrientedPlane3Factor, OrientedPlane3DirectionPrior\n",
|
|
"from gtsam.symbol_shorthand import X, P"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "factor_header_md"
|
|
},
|
|
"source": [
|
|
"## 1. `OrientedPlane3Factor`"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "factor_desc_md"
|
|
},
|
|
"source": [
|
|
"Connects a `Pose3` (camera/robot pose) and an `OrientedPlane3` (landmark). The measurement is the plane as observed *from the sensor frame*.\n",
|
|
"The error is calculated by transforming the global plane landmark into the sensor frame defined by the pose, and then computing the difference (`localCoordinates`) with the measured plane."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 3,
|
|
"metadata": {
|
|
"id": "factor_example_code"
|
|
},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"OrientedPlane3Factor: \n",
|
|
"OrientedPlane3Factor Factor (x0, p0)\n",
|
|
"Measured Plane : 0 0 1 1\n",
|
|
"isotropic dim=3 sigma=0.05\n",
|
|
"\n",
|
|
"Error at ground truth: 0.0\n",
|
|
"\n",
|
|
"Error with noisy plane: 0.6469041114912286\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"# Ground truth plane (e.g., z=1 in world frame)\n",
|
|
"gt_plane_world = OrientedPlane3(0, 0, 1, 1)\n",
|
|
"\n",
|
|
"# Ground truth pose\n",
|
|
"gt_pose = Pose3(Rot3.Yaw(0.1), Point3(0.5, 0, 0))\n",
|
|
"\n",
|
|
"# Measurement: transform the world plane into the camera frame\n",
|
|
"# measured_plane = gt_plane_world.transform(gt_pose)\n",
|
|
"# C++ header: Plane measurement z is a 4D vector [a,b,c,d] coefficients:\n",
|
|
"measured_plane_coeffs = gt_plane_world.planeCoefficients()\n",
|
|
"plane_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.05)\n",
|
|
"\n",
|
|
"pose_key = X(0)\n",
|
|
"plane_key = P(0)\n",
|
|
"\n",
|
|
"plane_factor = OrientedPlane3Factor(measured_plane_coeffs, plane_noise, pose_key, plane_key)\n",
|
|
"plane_factor.print(\"OrientedPlane3Factor: \")\n",
|
|
"\n",
|
|
"# Evaluate error\n",
|
|
"values = Values()\n",
|
|
"values.insert(pose_key, gt_pose)\n",
|
|
"values.insert(plane_key, gt_plane_world)\n",
|
|
"error1 = plane_factor.error(values)\n",
|
|
"print(f\"\\nError at ground truth: {error1}\")\n",
|
|
"\n",
|
|
"# Evaluate with slightly different plane estimate\n",
|
|
"noisy_plane = OrientedPlane3(0.01, 0.01, 0.99, 1.05)\n",
|
|
"values.update(plane_key, noisy_plane)\n",
|
|
"error2 = plane_factor.error(values)\n",
|
|
"print(f\"\\nError with noisy plane: {error2}\")"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "prior_header_md"
|
|
},
|
|
"source": [
|
|
"## 2. `OrientedPlane3DirectionPrior`"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "prior_desc_md"
|
|
},
|
|
"source": [
|
|
"A unary factor that puts a prior on the *direction* (normal vector) of an `OrientedPlane3`. The distance component (`d`) of the measured plane is ignored.\n",
|
|
"The error is the difference between the estimated plane's normal and the measured plane's normal, but as directions only have 2 DOF, the noise model also has to have dimension 2."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 4,
|
|
"metadata": {
|
|
"id": "prior_example_code"
|
|
},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"OrientedPlane3DirectionPrior: \n",
|
|
"OrientedPlane3DirectionPrior: Prior Factor on p0\n",
|
|
"Measured Plane : 0 0 1 0\n",
|
|
"isotropic dim=2 sigma=0.02\n",
|
|
"\n",
|
|
"Error for prior on noisy_plane: 0.2550239722533919\n",
|
|
"Error for prior on gt_plane_world: 0.0\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"# Measured direction prior (e.g., plane normal is close to world Z axis)\n",
|
|
"measured_prior_plane = OrientedPlane3(0, 0, 1, 0) # Distance (last coeff) is ignored\n",
|
|
"direction_noise = gtsam.noiseModel.Isotropic.Sigma(2, 0.02)\n",
|
|
"\n",
|
|
"prior_factor = OrientedPlane3DirectionPrior(plane_key, measured_prior_plane.planeCoefficients(), direction_noise)\n",
|
|
"prior_factor.print(\"OrientedPlane3DirectionPrior: \")\n",
|
|
"\n",
|
|
"# Evaluate error using the 'noisy_plane' from the previous step\n",
|
|
"error_prior = prior_factor.error(values) # values still contains plane_key -> noisy_plane\n",
|
|
"print(f\"\\nError for prior on noisy_plane: {error_prior}\")\n",
|
|
"\n",
|
|
"# Evaluate error for ground truth plane\n",
|
|
"values.update(plane_key, gt_plane_world)\n",
|
|
"error_prior_gt = prior_factor.error(values)\n",
|
|
"print(f\"Error for prior on gt_plane_world: {error_prior_gt}\")"
|
|
]
|
|
}
|
|
],
|
|
"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": 0
|
|
}
|