216 lines
8.1 KiB
Plaintext
216 lines
8.1 KiB
Plaintext
{
|
|
"cells": [
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "intro_md"
|
|
},
|
|
"source": [
|
|
"# PoseRotationPrior"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "desc_md"
|
|
},
|
|
"source": [
|
|
"`PoseRotationPrior<POSE>` is a unary factor that applies a prior constraint only to the **rotation** component of a `POSE` variable (e.g., `Pose2` or `Pose3`).\n",
|
|
"It ignores the translation component of the pose variable during error calculation.\n",
|
|
"The error is calculated as the difference between the rotation component of the pose variable and the measured prior rotation, expressed in the tangent space of the rotation group.\n",
|
|
"\n",
|
|
"Error: $ \\text{Log}(\\text{measured}^{-1} \\cdot \\text{pose.rotation}()) $\n",
|
|
"\n",
|
|
"This is useful when you have information about the absolute orientation of a pose but little or no information about its translation."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "colab_badge_md"
|
|
},
|
|
"source": [
|
|
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/PoseRotationPrior.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": {
|
|
"id": "pip_code",
|
|
"tags": [
|
|
"remove-cell"
|
|
]
|
|
},
|
|
"outputs": [],
|
|
"source": [
|
|
"%pip install --quiet gtsam-develop"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {
|
|
"id": "imports_code"
|
|
},
|
|
"outputs": [
|
|
{
|
|
"ename": "ImportError",
|
|
"evalue": "cannot import name 'PoseRotationPriorPose' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)",
|
|
"output_type": "error",
|
|
"traceback": [
|
|
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
|
|
"\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)",
|
|
"Cell \u001b[1;32mIn[2], line 3\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[1;32m----> 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m Pose3, Rot3, Point3, Values, PoseRotationPriorPose\n\u001b[0;32m 4\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 6\u001b[0m X \u001b[38;5;241m=\u001b[39m symbol_shorthand\u001b[38;5;241m.\u001b[39mX\n",
|
|
"\u001b[1;31mImportError\u001b[0m: cannot import name 'PoseRotationPriorPose' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"import gtsam\n",
|
|
"import numpy as np\n",
|
|
"from gtsam import Pose3, Rot3, Point3, Values, PoseRotationPriorPose3\n",
|
|
"from gtsam import symbol_shorthand\n",
|
|
"\n",
|
|
"X = symbol_shorthand.X"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "create_header_md"
|
|
},
|
|
"source": [
|
|
"## Creating a PoseRotationPrior"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "create_desc_md"
|
|
},
|
|
"source": [
|
|
"Provide the key of the pose variable, the measured prior rotation (`Rot3` for `Pose3`, `Rot2` for `Pose2`), and a noise model defined on the rotation manifold's dimension (e.g., 3 for `Rot3`)."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 1,
|
|
"metadata": {
|
|
"id": "create_example_code"
|
|
},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"PoseRotationPrior: PoseRotationPriorFactor on x0\n",
|
|
"Noise model: diagonal sigmas [0.05; 0.05; 0.05];\n",
|
|
"Measured Rotation R: [\n",
|
|
"\t0.707107, -0.707107, 0;\n",
|
|
"\t0.707107, 0.707107, 0;\n",
|
|
"\t0, 0, 1\n",
|
|
"]\n",
|
|
"\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"pose_key = X(0)\n",
|
|
"measured_rotation = Rot3.Yaw(np.pi / 4) # Prior belief about orientation\n",
|
|
"\n",
|
|
"# Noise model on rotation (3 dimensions for Rot3)\n",
|
|
"rotation_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.05) # 0.05 radians std dev\n",
|
|
"\n",
|
|
"# Factor type includes the Pose type, e.g. PoseRotationPriorPose3\n",
|
|
"factor = PoseRotationPriorPose3(pose_key, measured_rotation, rotation_noise)\n",
|
|
"factor.print(\"PoseRotationPrior: \")\n",
|
|
"\n",
|
|
"# Alternative constructor: extract rotation from a full Pose3 prior\n",
|
|
"full_pose_prior = Pose3(measured_rotation, Point3(10, 20, 30)) # Translation is ignored\n",
|
|
"factor_from_pose = PoseRotationPriorPose3(pose_key, full_pose_prior, rotation_noise)\n",
|
|
"# factor_from_pose.print(\"\\nFrom Pose Prior: \") # Should be identical"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "eval_header_md"
|
|
},
|
|
"source": [
|
|
"## Evaluating the Error"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "eval_desc_md"
|
|
},
|
|
"source": [
|
|
"The error depends only on the rotation part of the `Pose3` value in the `Values` object."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 2,
|
|
"metadata": {
|
|
"id": "eval_example_code"
|
|
},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"Error with correct rotation: [0. 0. 0.] (Should be near zero)\n",
|
|
"Error with incorrect rotation: [-0. -0. 2.00000004] (Should be non-zero)\n",
|
|
"Error with different translation: [-0. -0. 2.00000004] (Should be same as error2)\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"values = Values()\n",
|
|
"\n",
|
|
"# Pose with correct rotation but different translation\n",
|
|
"pose_val1 = Pose3(measured_rotation, Point3(1, 2, 3))\n",
|
|
"values.insert(pose_key, pose_val1)\n",
|
|
"error1 = factor.error(values)\n",
|
|
"print(f\"Error with correct rotation: {error1} (Should be near zero)\")\n",
|
|
"\n",
|
|
"# Pose with incorrect rotation\n",
|
|
"pose_val2 = Pose3(Rot3.Yaw(np.pi / 4 + 0.1), Point3(1, 2, 3))\n",
|
|
"values.update(pose_key, pose_val2)\n",
|
|
"error2 = factor.error(values)\n",
|
|
"print(f\"Error with incorrect rotation: {error2} (Should be non-zero)\")\n",
|
|
"\n",
|
|
"# Check that translation change doesn't affect error\n",
|
|
"pose_val3 = Pose3(Rot3.Yaw(np.pi / 4 + 0.1), Point3(100, 200, 300))\n",
|
|
"values.update(pose_key, pose_val3)\n",
|
|
"error3 = factor.error(values)\n",
|
|
"print(f\"Error with different translation: {error3} (Should be same as error2)\")\n",
|
|
"assert np.allclose(error2, error3)"
|
|
]
|
|
}
|
|
],
|
|
"metadata": {
|
|
"kernelspec": {
|
|
"display_name": "gtsam",
|
|
"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.13.1"
|
|
}
|
|
},
|
|
"nbformat": 4,
|
|
"nbformat_minor": 0
|
|
}
|