229 lines
7.3 KiB
Plaintext
229 lines
7.3 KiB
Plaintext
{
|
|
"cells": [
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "intro_md"
|
|
},
|
|
"source": [
|
|
"# Karcher Mean Factors"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "desc_md"
|
|
},
|
|
"source": [
|
|
"The [KarcherMeanFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/slam/KarcherMeanFactor.h) header provides functionality related to computing and constraining the Karcher mean (or Fréchet mean) of a set of rotations or other manifold values.\n",
|
|
"The Karcher mean $\\bar{R}$ of a set of rotations $\\{R_i\\}$ is the rotation that minimizes the sum of squared geodesic distances on the manifold:\n",
|
|
"$$ \\bar{R} = \\arg \\min_R \\sum_i d^2(R, R_i) = \\arg \\min_R \\sum_i || \\text{Log}(R_i^{-1} R) ||^2 $$\n",
|
|
"\n",
|
|
"Functions/Classes:\n",
|
|
"* `FindKarcherMean`: Computes the Karcher mean of a `std::vector` of rotations (or other suitable manifold type `T`). It solves the minimization problem above using a small internal optimization.\n",
|
|
"* `KarcherMeanFactor<T>`: A factor that enforces a constraint related to the Karcher mean. It does *not* constrain the mean to a specific value. Instead, it acts as a gauge fixing constraint by ensuring that the *sum of tangent space updates* applied to the variables involved sums to zero. This effectively removes the rotational degree of freedom corresponding to simultaneously rotating all variables."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "colab_badge_md"
|
|
},
|
|
"source": [
|
|
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/KarcherMeanFactor.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 Rot3, FindKarcherMeanRot3, KarcherMeanFactorRot3, Values\n",
|
|
"from gtsam.symbol_shorthand import R"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "find_header_md"
|
|
},
|
|
"source": [
|
|
"## 1. `FindKarcherMean`"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "find_desc_md"
|
|
},
|
|
"source": [
|
|
"Computes the Karcher mean of a list of rotations."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 3,
|
|
"metadata": {
|
|
"id": "find_example_code"
|
|
},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"Input Rotations (Yaw angles):\n",
|
|
" 0.100\n",
|
|
" 0.150\n",
|
|
" 0.050\n",
|
|
" 0.120\n",
|
|
"\n",
|
|
"Computed Karcher Mean (Yaw angle): 0.105\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"# Create a list of Rot3 objects\n",
|
|
"rotations = gtsam.Rot3Vector()\n",
|
|
"rotations.append(Rot3.Yaw(0.1))\n",
|
|
"rotations.append(Rot3.Yaw(0.15))\n",
|
|
"rotations.append(Rot3.Yaw(0.05))\n",
|
|
"rotations.append(Rot3.Yaw(0.12))\n",
|
|
"\n",
|
|
"# Compute the Karcher mean\n",
|
|
"karcher_mean = FindKarcherMeanRot3(rotations)\n",
|
|
"\n",
|
|
"print(\"Input Rotations (Yaw angles):\")\n",
|
|
"for r in rotations: print(f\" {r.yaw():.3f}\")\n",
|
|
"\n",
|
|
"print(f\"\\nComputed Karcher Mean (Yaw angle): {karcher_mean.yaw():.3f}\")\n",
|
|
"# Note: For yaw rotations, the Karcher mean yaw is close to the arithmetic mean (0.105)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "factor_header_md"
|
|
},
|
|
"source": [
|
|
"## 2. `KarcherMeanFactor<Rot3>`"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {
|
|
"id": "factor_desc_md"
|
|
},
|
|
"source": [
|
|
"Creates a factor that constrains the rotational average of a set of `Rot3` variables.\n",
|
|
"It acts as a soft gauge constraint. When linearized, it yields a Jacobian factor where each block corresponding to a variable is $\\sqrt{\\beta} I_{3x3}$, and the error vector is zero. The `beta` parameter (optional, defaults to 1) controls the strength (precision) of the constraint."
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 6,
|
|
"metadata": {
|
|
"id": "factor_example_code"
|
|
},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"KarcherMeanFactorRot3: keys = { r0 r1 r2 }\n",
|
|
"sqrt(beta): 10.0\n",
|
|
"\n",
|
|
"Jacobian for R(0):\n",
|
|
"[[10. 0. 0.]\n",
|
|
" [ 0. 10. 0.]\n",
|
|
" [ 0. 0. 10.]]\n",
|
|
"Jacobian for R(1):\n",
|
|
"[[10. 0. 0.]\n",
|
|
" [ 0. 10. 0.]\n",
|
|
" [ 0. 0. 10.]]\n",
|
|
"Jacobian for R(2):\n",
|
|
"[[10. 0. 0.]\n",
|
|
" [ 0. 10. 0.]\n",
|
|
" [ 0. 0. 10.]]\n",
|
|
"Error vector b:\n",
|
|
"[0. 0. 0.]\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"keys = [R(0), R(1), R(2)]\n",
|
|
"beta = 100.0 # Strength of the constraint\n",
|
|
"\n",
|
|
"k_factor = KarcherMeanFactorRot3(keys, 3, beta)\n",
|
|
"k_factor.print(\"KarcherMeanFactorRot3: \")\n",
|
|
"\n",
|
|
"# Linearization example\n",
|
|
"values = Values()\n",
|
|
"values.insert(R(0), Rot3.Yaw(0.1))\n",
|
|
"values.insert(R(1), Rot3.Yaw(0.2))\n",
|
|
"values.insert(R(2), Rot3.Yaw(0.3))\n",
|
|
"\n",
|
|
"linearized_factor = k_factor.linearize(values)\n",
|
|
"\n",
|
|
"# Check the Jacobian blocks (should be sqrt(beta)*Identity)\n",
|
|
"sqrt_beta = np.sqrt(beta)\n",
|
|
"A = linearized_factor.getA()\n",
|
|
"assert A.shape == (3, 9), f\"Unexpected shape for A: {A.shape}\"\n",
|
|
"A0 = A[:, :3]\n",
|
|
"A1 = A[:, 3:6]\n",
|
|
"A2 = A[:, 6:9]\n",
|
|
"b = linearized_factor.getb()\n",
|
|
"\n",
|
|
"print(f\"sqrt(beta): {sqrt_beta}\")\n",
|
|
"print(f\"\\nJacobian for R(0):\\n{A0}\")\n",
|
|
"print(f\"Jacobian for R(1):\\n{A1}\")\n",
|
|
"print(f\"Jacobian for R(2):\\n{A2}\")\n",
|
|
"print(f\"Error vector b:\\n{b}\")"
|
|
]
|
|
}
|
|
],
|
|
"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
|
|
}
|