258 lines
8.4 KiB
Plaintext
258 lines
8.4 KiB
Plaintext
{
|
|
"cells": [
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "intro_md"
|
|
},
|
|
"source": [
|
|
"# Frobenius Norm Factors"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "desc_md"
|
|
},
|
|
"source": [
|
|
"This header defines factors that operate directly on the entries of rotation matrices (`Rot3` or generally `SO(n)`) rather than using their Lie algebra representation (log map). They minimize the Frobenius norm of the difference between rotation matrices.\n",
|
|
"\n",
|
|
"These factors can sometimes be useful in specific optimization contexts, particularly in rotation averaging problems or as alternatives to standard `BetweenFactor` or `PriorFactor` on rotations.\n",
|
|
"\n",
|
|
"* `FrobeniusPrior<T>`: Penalizes the Frobenius norm difference between a variable rotation `T` and a fixed target matrix `M`. Error is $||T - M||_F^2$.\n",
|
|
"* `FrobeniusFactor<T>`: Penalizes the Frobenius norm difference between two variable rotations `T1` and `T2`. Error is $||T1 - T2||_F^2$.\n",
|
|
"* `FrobeniusBetweenFactor<T>`: Penalizes the Frobenius norm difference between the predicted rotation `T2` and the expected rotation `T1 * T12_measured`. Error is $||T1 \\cdot T_{12} - T2||_F^2$.\n",
|
|
"**Note:** The noise models for these factors operate on the vectorized rotation matrix (e.g., 9 elements for `Rot3`). The helper function `ConvertNoiseModel` attempts to convert standard rotation noise models (like those for `BetweenFactor<Rot3>`) into an appropriate isotropic noise model for the Frobenius factor. It expects the input noise model to be isotropic."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "colab_badge_md"
|
|
},
|
|
"source": [
|
|
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/FrobeniusFactor.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": 1,
|
|
"metadata": {
|
|
"id": "imports_code"
|
|
},
|
|
"outputs": [],
|
|
"source": [
|
|
"import gtsam\n",
|
|
"import numpy as np\n",
|
|
"from gtsam import Rot3, Pose3, Values\n",
|
|
"from gtsam import FrobeniusPriorRot3, FrobeniusFactorRot3, FrobeniusBetweenFactorRot3\n",
|
|
"from gtsam import symbol_shorthand\n",
|
|
"\n",
|
|
"X = symbol_shorthand.X\n",
|
|
"R = symbol_shorthand.R # Using 'R' for Rot3 keys"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "fprior_header_md"
|
|
},
|
|
"source": [
|
|
"## 1. `FrobeniusPrior<Rot3>`"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "fprior_desc_md"
|
|
},
|
|
"source": [
|
|
"Constrains a `Rot3` variable `R(0)` to be close to a target matrix `M` in the Frobenius norm sense."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 4,
|
|
"metadata": {
|
|
"id": "fprior_example_code"
|
|
},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"FrobeniusPriorRot3: keys = { r0 }\n",
|
|
" noise model: unit (9) \n",
|
|
"\n",
|
|
"FrobeniusPrior error (vectorized matrix diff): 9.999916666944462e-05\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"target_matrix = Rot3.Yaw(0.1).matrix() # Target matrix (must be 3x3)\n",
|
|
"key = R(0)\n",
|
|
"\n",
|
|
"# Create a standard isotropic noise model for rotation (3 dimensional)\n",
|
|
"rot_noise_model = gtsam.noiseModel.Isotropic.Sigma(3, 0.01)\n",
|
|
"\n",
|
|
"# Convert it for the Frobenius factor (9 dimensional)\n",
|
|
"frobenius_noise_model_prior = gtsam.noiseModel.Isotropic.Sigma(9, 0.01) # Or use ConvertNoiseModel\n",
|
|
"\n",
|
|
"prior_fro = FrobeniusPriorRot3(key, target_matrix, frobenius_noise_model_prior)\n",
|
|
"prior_fro.print(\"FrobeniusPriorRot3: \")\n",
|
|
"\n",
|
|
"# Evaluate error\n",
|
|
"values = Values()\n",
|
|
"values.insert(key, Rot3.Yaw(0.11)) # Slightly different rotation\n",
|
|
"error_prior = prior_fro.error(values)\n",
|
|
"print(f\"\\nFrobeniusPrior error (vectorized matrix diff): {error_prior}\")"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "ffactor_header_md"
|
|
},
|
|
"source": [
|
|
"## 1. `FrobeniusFactor<Rot3>`"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "ffactor_desc_md"
|
|
},
|
|
"source": [
|
|
"Constrains two `Rot3` variables `R(0)` and `R(1)` to be close to each other in the Frobenius norm sense."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 7,
|
|
"metadata": {
|
|
"id": "ffactor_example_code"
|
|
},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"\n",
|
|
"FrobeniusFactorRot3: keys = { r0 r1 }\n",
|
|
" noise model: unit (9) \n",
|
|
"\n",
|
|
"FrobeniusFactor error (vectorized matrix diff): 2.499994791671017e-05\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"values = Values()\n",
|
|
"\n",
|
|
"key1 = R(0)\n",
|
|
"key2 = R(1)\n",
|
|
"# Use same noise model dimension (9)\n",
|
|
"frobenius_noise_model_between = gtsam.noiseModel.Isotropic.Sigma(9, 0.02)\n",
|
|
"\n",
|
|
"factor_fro = FrobeniusFactorRot3(key1, key2, frobenius_noise_model_between)\n",
|
|
"factor_fro.print(\"\\nFrobeniusFactorRot3: \")\n",
|
|
"\n",
|
|
"# Evaluate error\n",
|
|
"values.insert(key1, Rot3.Yaw(0.11))\n",
|
|
"values.insert(key2, Rot3.Yaw(0.115)) # R1 slightly different from R0\n",
|
|
"error_factor = factor_fro.error(values)\n",
|
|
"print(f\"\\nFrobeniusFactor error (vectorized matrix diff): {error_factor}\")"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "fbetween_header_md"
|
|
},
|
|
"source": [
|
|
"## 3. `FrobeniusBetweenFactor<Rot3>`"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "fbetween_desc_md"
|
|
},
|
|
"source": [
|
|
"Acts like `BetweenFactor<Rot3>` but minimizes $||R_1 \\cdot R_{12} - R_2||_F^2$ instead of using the Log map error."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 9,
|
|
"metadata": {
|
|
"id": "fbetween_example_code"
|
|
},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"\n",
|
|
"FrobeniusBetweenFactorRot3: FrobeniusBetweenFactor<class gtsam::Rot3>(r0,r1)\n",
|
|
" T12: [\n",
|
|
"\t0.999988, -0.00499998, 0;\n",
|
|
"\t0.00499998, 0.999988, 0;\n",
|
|
"\t0, 0, 1\n",
|
|
"]\n",
|
|
" noise model: unit (9) \n",
|
|
"\n",
|
|
"FrobeniusBetweenFactor error: 1.925929944387236e-34\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"measured_R12 = Rot3.Yaw(0.005)\n",
|
|
"# Use same noise model dimension (9)\n",
|
|
"frobenius_noise_model_b = gtsam.noiseModel.Isotropic.Sigma(9, 0.005)\n",
|
|
"\n",
|
|
"between_fro = FrobeniusBetweenFactorRot3(key1, key2, measured_R12, frobenius_noise_model_b)\n",
|
|
"between_fro.print(\"\\nFrobeniusBetweenFactorRot3: \")\n",
|
|
"\n",
|
|
"# Evaluate error (uses R(0)=Yaw(0.11), R(1)=Yaw(0.115))\n",
|
|
"error_between = between_fro.error(values)\n",
|
|
"print(f\"\\nFrobeniusBetweenFactor error: {error_between}\")"
|
|
]
|
|
}
|
|
],
|
|
"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
|
|
}
|