-
Notifications
You must be signed in to change notification settings - Fork 2.5k
Add Doppler ICP Registration Documentation #7425
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Open
heethesh
wants to merge
2
commits into
isl-org:main
Choose a base branch
from
heethesh:heethesh/doppler-icp-docs
base: main
Could not load branches
Branch not found: {{ refName }}
Loading
Could not load tags
Nothing to show
Loading
Are you sure you want to change the base?
Some commits from the old base branch may be removed from the timeline,
and old review comments may become outdated.
+300
−7
Open
Changes from all commits
Commits
Show all changes
2 commits
Select commit
Hold shift + click to select a range
File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change | ||||
|---|---|---|---|---|---|---|
|
|
@@ -251,6 +251,11 @@ | |||||
| " - Colored ICP.\n", | ||||||
| " - Requires `target` point-cloud to have `normals` attribute (of same dtype as `position` attribute).\n", | ||||||
| " - Requires `source` and `target` point-clouds to have `colors` attribute (of same dtype as `position` attribute).\n", | ||||||
| "- **o3d.t.pipelines.registration.TransformationEstimationForDopplerICP(period, lambda_doppler, ...)**\n", | ||||||
| " - Doppler ICP.\n", | ||||||
| " - Requires `target` point-cloud to have `normals` attribute (of same dtype as `position` attribute).\n", | ||||||
| " - Requires `source` point-cloud to have `dopplers` and `directions` attributes (of same dtype as `position` attribute).\n", | ||||||
| " - Requires calibration parameters: `period` (time between scans) and `transform_vehicle_to_sensor` (4x4 transformation matrix).\n", | ||||||
| "- **o3d.t.pipelines.registration.TransformationEstimationForGeneralizedICP(robust_kernel, epsilon)** [To be added].\n", | ||||||
| " - Generalized ICP.\n" | ||||||
| ] | ||||||
|
|
@@ -383,7 +388,7 @@ | |||||
| "# Example callback_after_iteration lambda function:\n", | ||||||
| "callback_after_iteration = lambda updated_result_dict : print(\"Iteration Index: {}, Fitness: {}, Inlier RMSE: {},\".format(\n", | ||||||
| " updated_result_dict[\"iteration_index\"].item(),\n", | ||||||
| " updated_result_dict[\"fitness\"].item(), \n", | ||||||
| " updated_result_dict[\"fitness\"].item(),\n", | ||||||
| " updated_result_dict[\"inlier_rmse\"].item()))" | ||||||
| ] | ||||||
| }, | ||||||
|
|
@@ -645,10 +650,10 @@ | |||||
| "\n", | ||||||
| "# Save iteration wise `fitness`, `inlier_rmse`, etc. to analyse and tune result.\n", | ||||||
| "callback_after_iteration = lambda loss_log_map : print(\"Iteration Index: {}, Scale Index: {}, Scale Iteration Index: {}, Fitness: {}, Inlier RMSE: {},\".format(\n", | ||||||
| " loss_log_map[\"iteration_index\"].item(), \n", | ||||||
| " loss_log_map[\"scale_index\"].item(), \n", | ||||||
| " loss_log_map[\"scale_iteration_index\"].item(), \n", | ||||||
| " loss_log_map[\"fitness\"].item(), \n", | ||||||
| " loss_log_map[\"iteration_index\"].item(),\n", | ||||||
| " loss_log_map[\"scale_index\"].item(),\n", | ||||||
| " loss_log_map[\"scale_iteration_index\"].item(),\n", | ||||||
| " loss_log_map[\"fitness\"].item(),\n", | ||||||
| " loss_log_map[\"inlier_rmse\"].item()))" | ||||||
| ] | ||||||
| }, | ||||||
|
|
@@ -1261,6 +1266,293 @@ | |||||
| "\n", | ||||||
| "draw_registration_result(source, target, reg_multiscale_icp.transformation)" | ||||||
| ] | ||||||
| }, | ||||||
| { | ||||||
| "cell_type": "markdown", | ||||||
| "metadata": {}, | ||||||
| "source": [ | ||||||
| "---" | ||||||
| ] | ||||||
| }, | ||||||
| { | ||||||
| "cell_type": "markdown", | ||||||
| "metadata": {}, | ||||||
| "source": [ | ||||||
| "## Doppler ICP Registration\n", | ||||||
| "This tutorial demonstrates an ICP variant that uses both geometry and Doppler velocity measurements for registration. It implements the algorithm of [\\[Hexsel2022\\]](https://arxiv.org/abs/2201.11944). Doppler ICP leverages instantaneous radial velocity measurements from range sensors (such as FMCW LiDAR) to improve registration accuracy and achieve faster convergence compared to classical point-to-plane ICP. The Doppler information provides additional constraints that help disambiguate motion, making this algorithm more accurate, robust, and faster-converging than geometric-only ICP methods, especially in feature-denied environments like hallways, tunnels, highways, and bridges. " | ||||||
| ] | ||||||
| }, | ||||||
| { | ||||||
| "cell_type": "markdown", | ||||||
| "metadata": {}, | ||||||
| "source": [ | ||||||
| "" | ||||||
| ] | ||||||
| }, | ||||||
| { | ||||||
| "cell_type": "markdown", | ||||||
| "metadata": {}, | ||||||
| "source": [ | ||||||
| "### Understanding Doppler ICP\n", | ||||||
| "\n", | ||||||
| "Doppler ICP combines geometric and Doppler velocity constraints in a joint optimization objective:\n", | ||||||
| "\n", | ||||||
| "\\begin{equation}\n", | ||||||
| "E(\\mathbf{T}) = (1-\\lambda)E_{G}(\\mathbf{T}) + \\lambda E_{D}(\\mathbf{T})\n", | ||||||
| "\\end{equation}\n", | ||||||
| "\n", | ||||||
| "where $\\mathbf{T}$ is the transformation matrix to be estimated, $\\lambda \\in [0,1]$ is a weight parameter, $E_{G}$ is the geometric term, and $E_{D}$ is the Doppler term.\n", | ||||||
| "\n", | ||||||
| "The geometric term $E_{G}$ is the same as the [Point-to-plane ICP](../pipelines/icp_registration.ipynb#Point-to-plane-ICP) objective:\n", | ||||||
| "\n", | ||||||
| "\\begin{equation}\n", | ||||||
| "E_{G}(\\mathbf{T}) = \\sum_{(\\mathbf{p},\\mathbf{q})\\in\\mathcal{K}}\\big((\\mathbf{p} - \\mathbf{T}\\mathbf{q})\\cdot\\mathbf{n}_{\\mathbf{p}}\\big)^{2},\n", | ||||||
| "\\end{equation}\n", | ||||||
| "\n", | ||||||
| "where $\\mathcal{K}$ is the correspondence set and $\\mathbf{n}_{\\mathbf{p}}$ is the normal of point $\\mathbf{p}$.\n", | ||||||
| "\n", | ||||||
| "The Doppler term $E_{D}$ measures the consistency between the measured Doppler velocity and the velocity predicted by the estimated transformation:\n", | ||||||
| "\n", | ||||||
| "\\begin{equation}\n", | ||||||
| "E_{D}(\\mathbf{T}) = \\sum_{(\\mathbf{p},\\mathbf{q})\\in\\mathcal{K}}\\big(d_{\\mathbf{q}} - \\mathbf{v}(\\mathbf{T})\\cdot\\hat{\\mathbf{r}}_{\\mathbf{q}}\\big)^{2},\n", | ||||||
| "\\end{equation}\n", | ||||||
| "\n", | ||||||
| "where $d_{\\mathbf{q}}$ is the measured Doppler velocity at point $\\mathbf{q}$, $\\mathbf{v}(\\mathbf{T})$ is the velocity implied by transformation $\\mathbf{T}$, and $\\hat{\\mathbf{r}}_{\\mathbf{q}}$ is the unit direction vector from the sensor to point $\\mathbf{q}$.\n", | ||||||
| "\n", | ||||||
| "By jointly optimizing these complementary constraints, Doppler ICP achieves **faster convergence** compared to classical point-to-plane ICP that solely relies on geometric residuals. The Doppler term provides additional information that helps guide the optimization toward the correct solution more efficiently, particularly in feature-denied environments where geometry alone is ambiguous.\n", | ||||||
| "\n", | ||||||
| "For more details, refer to [\\[Hexsel2022\\]](https://arxiv.org/abs/2201.11944)." | ||||||
| ] | ||||||
| }, | ||||||
| { | ||||||
| "cell_type": "markdown", | ||||||
| "metadata": {}, | ||||||
| "source": [ | ||||||
| "### Requirements\n", | ||||||
| "\n", | ||||||
| "Doppler ICP requires:\n", | ||||||
| "\n", | ||||||
| "- **Source point cloud** with:\n", | ||||||
| " - `dopplers` attribute: Measured Doppler or radial velocities for each point.\n", | ||||||
| " - `directions` attribute: Unit direction vectors from sensor to each point (normalized point positions). The direction vectors must correspond the raw range measurements (without any motion compensation or undistortion applied).\n", | ||||||
| " \n", | ||||||
| "- **Target point cloud** with:\n", | ||||||
| " - `normals` attribute: Surface normals for each point.\n", | ||||||
| " \n", | ||||||
| "- **Calibration parameters**:\n", | ||||||
| " - `period`: Time period (in seconds) between source and target scans.\n", | ||||||
| " - `transform_vehicle_to_sensor`: 4x4 transformation matrix from vehicle frame to sensor frame. This is optional." | ||||||
| ] | ||||||
| }, | ||||||
| { | ||||||
| "cell_type": "code", | ||||||
| "execution_count": null, | ||||||
| "metadata": {}, | ||||||
| "outputs": [], | ||||||
| "source": [ | ||||||
| "print(\"1. Load Doppler ICP dataset\")\n", | ||||||
| "import json\n", | ||||||
| "\n", | ||||||
| "demo_sequence = o3d.data.DemoDopplerICPSequence()\n", | ||||||
| "\n", | ||||||
| "# Load point clouds\n", | ||||||
| "source_idx = 0\n", | ||||||
| "target_idx = 1\n", | ||||||
| "source = o3d.t.io.read_point_cloud(demo_sequence.paths[source_idx])\n", | ||||||
| "target = o3d.t.io.read_point_cloud(demo_sequence.paths[target_idx])\n", | ||||||
| "\n", | ||||||
| "# Load calibration parameters\n", | ||||||
| "with open(demo_sequence.calibration_path) as f:\n", | ||||||
| " calibration_data = json.load(f)\n", | ||||||
| "\n", | ||||||
| "transform_vehicle_to_sensor = np.array(\n", | ||||||
| " calibration_data['transform_vehicle_to_sensor']).reshape(4, 4)\n", | ||||||
| "period = calibration_data['period']\n", | ||||||
| "\n", | ||||||
| "print(f\"Loaded source: {demo_sequence.paths[source_idx]}\")\n", | ||||||
| "print(f\"Loaded target: {demo_sequence.paths[target_idx]}\")\n", | ||||||
| "print(f\"Time period between scans: {period:.4f} seconds\")" | ||||||
| ] | ||||||
| }, | ||||||
| { | ||||||
| "cell_type": "code", | ||||||
| "execution_count": null, | ||||||
| "metadata": {}, | ||||||
| "outputs": [], | ||||||
| "source": [ | ||||||
| "print(\"2. Prepare point clouds for Doppler ICP\")\n", | ||||||
| "\n", | ||||||
| "# Setup device\n", | ||||||
| "dtype = o3d.core.float32\n", | ||||||
| "device = o3d.core.Device('CPU:0')\n", | ||||||
| "\n", | ||||||
| "# Downsample point clouds\n", | ||||||
| "source_in_S = source.uniform_down_sample(5)\n", | ||||||
| "target_in_S = target.uniform_down_sample(5)\n", | ||||||
| "\n", | ||||||
| "# Transform from sensor frame (S) to vehicle frame (V)\n", | ||||||
| "source_in_V = source_in_S.to(device).transform(transform_vehicle_to_sensor)\n", | ||||||
| "target_in_V = target_in_S.to(device).transform(transform_vehicle_to_sensor)\n", | ||||||
| "\n", | ||||||
| "# Compute normals for target (required for point-to-plane ICP)\n", | ||||||
| "target_in_V.estimate_normals(radius=10.0, max_nn=30)\n", | ||||||
| "\n", | ||||||
| "# Compute direction vectors for source (required for Doppler ICP)\n", | ||||||
| "# Directions are unit vectors from sensor to each point (in sensor frame)\n", | ||||||
| "directions = source_in_S.point.positions.numpy()\n", | ||||||
| "norms = np.tile(np.linalg.norm(directions, axis=1), (3, 1)).T\n", | ||||||
| "directions = directions / norms\n", | ||||||
| "source_in_V.point['directions'] = o3d.core.Tensor(directions, dtype, device)\n", | ||||||
| "\n", | ||||||
| "# Note: The source point cloud should already have 'dopplers' attribute\n", | ||||||
| "# from the dataset. If not, you would need to add it here.\n", | ||||||
| "print(\"Source point cloud attributes:\", source_in_V.point.keys())\n", | ||||||
| "print(\"Target point cloud attributes:\", target_in_V.point.keys())" | ||||||
| ] | ||||||
| }, | ||||||
| { | ||||||
| "cell_type": "markdown", | ||||||
| "metadata": {}, | ||||||
| "source": [ | ||||||
| "### Setting baseline with point-to-plane registration\n", | ||||||
| "\n", | ||||||
| "We first run Point-to-plane ICP as a baseline approach for comparison." | ||||||
| ] | ||||||
| }, | ||||||
| { | ||||||
| "cell_type": "code", | ||||||
| "execution_count": null, | ||||||
| "metadata": {}, | ||||||
| "outputs": [], | ||||||
| "source": [ | ||||||
| "# Setup robust kernel\n", | ||||||
| "kernel = treg.robust_kernel.RobustKernel(\n", | ||||||
| " treg.robust_kernel.RobustKernelMethod.TukeyLoss,\n", | ||||||
| " scaling_parameter=0.5)\n", | ||||||
| "\n", | ||||||
| "# Setup convergence criteria\n", | ||||||
| "criteria = treg.ICPConvergenceCriteria(\n", | ||||||
| " relative_fitness=1e-6,\n", | ||||||
| " relative_rmse=1e-6,\n", | ||||||
| " max_iteration=200)\n", | ||||||
| "\n", | ||||||
| "# Point-to-plane ICP estimator\n", | ||||||
| "estimator_p2l = treg.TransformationEstimationPointToPlane(kernel)\n", | ||||||
| "\n", | ||||||
| "# Initial transformation\n", | ||||||
| "init_transform = o3d.core.Tensor(np.eye(4), device=device, dtype=dtype)\n", | ||||||
| "\n", | ||||||
| "# Run point-to-plane ICP\n", | ||||||
| "max_neighbor_distance = 0.3\n", | ||||||
| "print(\"Apply Point-to-Plane ICP\")\n", | ||||||
| "s = time.time()\n", | ||||||
| "\n", | ||||||
| "reg_p2l = treg.icp(source_in_V, target_in_V, max_neighbor_distance,\n", | ||||||
| " init_transform, estimator_p2l, criteria)\n", | ||||||
| "\n", | ||||||
| "p2l_time = time.time() - s\n", | ||||||
| "print(f\"Time taken by Point-to-Plane ICP: {p2l_time:.4f} seconds\")\n", | ||||||
| "print(f\"Fitness: {reg_p2l.fitness:.6f}\")\n", | ||||||
| "print(f\"Inlier RMSE: {reg_p2l.inlier_rmse:.6f}\")\n", | ||||||
| "print(f\"Iterations: {reg_p2l.num_iterations}\")" | ||||||
| ] | ||||||
| }, | ||||||
| { | ||||||
| "cell_type": "markdown", | ||||||
| "metadata": {}, | ||||||
| "source": [ | ||||||
| "### Doppler ICP Registration\n", | ||||||
| "\n", | ||||||
| "Now we run Doppler ICP using the same parameters. The Doppler term provides additional constraints that help improve registration accuracy and achieve faster convergence compared to point-to-plane ICP.\n", | ||||||
| "\n", | ||||||
| "The `TransformationEstimationForDopplerICP` class provides several parameters to control the registration behavior:\n", | ||||||
| "\n", | ||||||
| "#### Core Parameters\n", | ||||||
| "\n", | ||||||
| "- **`period`** (double, default: 0.1): Time period in seconds between the source and target point clouds. This is used to compute the velocity implied by the transformation. For sequential scans, multiply the base period by the frame difference (e.g., `period * (target_idx - source_idx)`).\n", | ||||||
| "\n", | ||||||
| "- **`lambda_doppler`** (double, default: 0.01): Weight parameter `λ ∈ [0, 1]` that balances the geometric and Doppler terms in the objective function `(1-λ)E_G + λE_D`. A smaller value (e.g., 0.01) emphasizes geometric constraints, while a larger value increases the influence of Doppler measurements. Emprirically 0.01 is a good default value.\n", | ||||||
|
||||||
| "- **`lambda_doppler`** (double, default: 0.01): Weight parameter `λ ∈ [0, 1]` that balances the geometric and Doppler terms in the objective function `(1-λ)E_G + λE_D`. A smaller value (e.g., 0.01) emphasizes geometric constraints, while a larger value increases the influence of Doppler measurements. Emprirically 0.01 is a good default value.\n", | |
| "- **`lambda_doppler`** (double, default: 0.01): Weight parameter `λ ∈ [0, 1]` that balances the geometric and Doppler terms in the objective function `(1-λ)E_G + λE_D`. A smaller value (e.g., 0.01) emphasizes geometric constraints, while a larger value increases the influence of Doppler measurements. Empirically 0.01 is a good default value.\n", |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Missing 'to' between 'correspond' and 'the'. Should be 'must correspond to the raw range measurements'.