Skip to content

Add an optimization tutorial for solving point cloud registration #288

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.

Sign up for GitHub

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
xipengwang wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Add an optimization tutorial for solving point cloud registration #288

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
376 changes: 376 additions & 0 deletions notebooks/tutorials/optimization_tutorial.ipynb
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,382 @@
"\n",
"For examples of how to run optimization from C++, see `symforce/examples/bundle_adjustment_in_the_large` and `symforce/examples/bundle_adjustment`."
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"## Point cloud regisration problem\n",
"Let's walk through solving a simplified [point cloud registration problem](https://en.wikipedia.org/wiki/Point-set_registration) with Symforce. In this example, we know the correspondences between two point clouds and we would like to find a `sf.Pose3` to align two point clouds."
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"### Symforce setup "
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"In this example, we will use symbolic variables to define our residual function so we need to import `symforce.symbolic`."
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [],
"source": [
"import symforce\n",
"symforce.set_symbolic_api(\"sympy\")\n",
"symforce.set_log_level(\"warning\")\n",
"symforce.set_epsilon_to_symbol()\n",
"\n",
"import symforce.symbolic as sf"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We build up input `Values` using runtime types, e.g. `sym.pose3`, `np.ndarray` instead of symbolic types."
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
"from symforce.values import Values\n",
"import numpy as np\n",
"import sym"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"For building a factor graph and solve the problem, we need to import `Factor` and `Optimizer`."
]
},
{
"cell_type": "code",
"execution_count": 10,
"metadata": {},
"outputs": [],
"source": [
"from symforce.opt.factor import Factor\n",
"from symforce.opt.optimizer import Optimizer"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"In this example, we will also show the usage of customized cost functions for computing residuals. "
]
},
{
"cell_type": "code",
"execution_count": 9,
"metadata": {},
"outputs": [],
"source": [
"from symforce.opt.noise_models import IsotropicNoiseModel"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"### Set up the problem"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"We first generate 30 points in robot body frame."
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [],
"source": [
"np.random.seed(seed=1024)\n",
"num_points = 30\n",
"points_b = []\n",
"for i in range(num_points):\n",
" points_b.append(np.random.uniform(low=0.0, high=1.0, size=(3,)))"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"We create a ground truth `w_T_b_GT`, and use it to transform the 30 points in body frame to world frame. "
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {},
"outputs": [],
"source": [
"pose_tangent_delta = np.array([0.1, 0.1, 0, 0, 0, 10])\n",
"w_T_b_GT = sym.Pose3.identity().retract(pose_tangent_delta)\n",
"points_w = []\n",
"for i in range(num_points):\n",
" points_w.append(w_T_b_GT * points_b[i])"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"### Build an optimization problem"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We define a simple residual function which simply calculates the difference between two corresponded points in the aligned point clouds. Note that we apply a `IsotropicNoiseModel` to whiten the residual. It doesn't really affect the optimization results but you can see the final total error is scaled by the `sigma`. "
]
},
{
"cell_type": "code",
"execution_count": 6,
"metadata": {},
"outputs": [],
"source": [
"def projection_residual(w_T_b: sf.Pose3, b_t_p: sf.V3, w_t_p: sf.V3, sigma: sf.Scalar) -> sf.V3:\n",
" noise_model = IsotropicNoiseModel.from_sigma(sigma)\n",
" return noise_model.whiten(w_T_b*b_t_p - w_t_p)"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"We build up the inputs to the optimization problem using symforce runtime types. Note that the initial pose value is `sym.Pose3.identity()` which is different from `w_T_b_GT`. We hope the optimizer can give us a solution very close to `w_T_b_GT`."
]
},
{
"cell_type": "code",
"execution_count": 7,
"metadata": {},
"outputs": [],
"source": [
"inputs = Values(\n",
" w_T_b=sym.Pose3.identity(),\n",
" points_b=points_b,\n",
" points_w=points_w,\n",
" sigma=10.0,\n",
")"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"Create `Factor`s from the residual functions and a set of keys."
]
},
{
"cell_type": "code",
"execution_count": 11,
"metadata": {},
"outputs": [],
"source": [
"factors = []\n",
"for i in range(num_points):\n",
" factors.append(\n",
" Factor(\n",
" residual=projection_residual,\n",
" keys=[\"w_T_b\", f\"points_b[{i}]\", f\"points_w[{i}]\", \"sigma\"],\n",
" )\n",
" )"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"### Solve the optimization problem"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"We create an Optimizer with factors and tell it to only optimize the pose key (the rest are held constant). Here we show lots of configure parameters for the optimizer you can tune."
]
},
{
"cell_type": "code",
"execution_count": 12,
"metadata": {},
"outputs": [],
"source": [
"optimizer = Optimizer(\n",
" factors=factors,\n",
" optimized_keys=[\"w_T_b\"],\n",
" debug_stats=False,\n",
" params=Optimizer.Params(\n",
" verbose = False,\n",
" initial_lambda = 1.0,\n",
" lambda_up_factor = 4.0,\n",
" lambda_down_factor = 1 / 4.0,\n",
" lambda_lower_bound = 0.0,\n",
" lambda_upper_bound = 10000.0,\n",
" use_diagonal_damping = False,\n",
" use_unit_damping = False,\n",
" keep_max_diagonal_damping = False,\n",
" diagonal_damping_min = 1e-6,\n",
" iterations = 500,\n",
" early_exit_min_reduction = 1e-6,\n",
" enable_bold_updates = False,\n",
" )\n",
")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Run the optimization! This returns an `Optimizer.Result` object that contains the optimized values and many other things."
]
},
{
"cell_type": "code",
"execution_count": 14,
"metadata": {},
"outputs": [],
"source": [
"results = optimizer.optimize(inputs)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We can print out the pose before and after the optimization and compare it against our `w_T_b_GT`."
]
},
{
"cell_type": "code",
"execution_count": 15,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Early exist? : True\n",
"w_T_b_GT: \n"
]
},
{
"data": {
"text/plain": [
"<Pose3 [0.04995834374876001, 0.04995834374876001, 0.0, 0.9975010414930711, 0.0, 0.0, 10.0]>"
]
},
"metadata": {},
"output_type": "display_data"
},
{
"name": "stdout",
"output_type": "stream",
"text": [
"Initial w_T_b: \n"
]
},
{
"data": {
"text/plain": [
"<Pose3 [0, 0, 0, 1, 0, 0, 0]>"
]
},
"metadata": {},
"output_type": "display_data"
},
{
"name": "stdout",
"output_type": "stream",
"text": [
"Optimized w_T_b: \n"
]
},
{
"data": {
"text/plain": [
"<Pose3 [0.049958343748759994, 0.04995834374875999, 5.6387511044753574e-18, 0.997501041493071, 3.670711373690943e-17, -4.4328076914926344e-17, 10.0]>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"print(f\"Early exist? : {results.early_exited}\")\n",
"print(\"w_T_b_GT: \")\n",
"display(w_T_b_GT)\n",
"print(\"Initial w_T_b: \")\n",
"display(results.initial_values.attr.w_T_b)\n",
"print(\"Optimized w_T_b: \")\n",
"display(results.optimized_values.attr.w_T_b)"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"We can also print out the total error before and after optimization. Try to change the `sigma` to see how it will change the error value. "
]
},
{
"cell_type": "code",
"execution_count": 16,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Initial error: 14.95341695550227\n",
"Final error: 1.2710806613746568e-31\n"
]
}
],
"source": [
"initial_linearization = optimizer.linearize(results.initial_values)\n",
"print(f\"Initial error: {initial_linearization.error()}\")\n",
"\n",
"optimized_linearization = optimizer.linearize(results.optimized_values)\n",
"print(f\"Final error: {optimized_linearization.error()}\")"
]
}
],
"metadata": {
Expand Down