From ecf1fdb846056e3b46951b6bae56cc0afb0066ae Mon Sep 17 00:00:00 2001 From: mfourmy Date: Wed, 6 Dec 2023 17:50:26 +0100 Subject: [PATCH] Revive megapose visualization notebook --- .../megapose_estimator_visualization.ipynb | 1859 ++++++++--------- 1 file changed, 883 insertions(+), 976 deletions(-) diff --git a/notebooks/megapose/megapose_estimator_visualization.ipynb b/notebooks/megapose/megapose_estimator_visualization.ipynb index be23a8b5..9967e458 100644 --- a/notebooks/megapose/megapose_estimator_visualization.ipynb +++ b/notebooks/megapose/megapose_estimator_visualization.ipynb @@ -1,978 +1,885 @@ { - "cells": [ - { - "cell_type": "code", - "execution_count": null, - "id": "d3874bde-0969-418e-9e3d-4cf9ab930413", - "metadata": {}, - "outputs": [], - "source": [ - "%load_ext autoreload\n", - "%autoreload 2\n", - "\n", - "import os\n", - "import copy\n", - "import time\n", - "import torch\n", - "import numpy as np\n", - "import random\n", - "import transforms3d\n", - "\n", - "\n", - "from omegaconf import OmegaConf\n", - "\n", - "import happypose.pose_estimators.megapose\n", - "\n", - "from happypose.toolbox.datasets.datasets_cfg import make_scene_dataset\n", - "from happypose.pose_estimators.megapose.config import LOCAL_DATA_DIR, NB_DATA_DIR\n", - "from happypose.pose_estimators.megapose.training.utils import RGB_DIMS\n", - "from happypose.pose_estimators.megapose.inference.utils import make_cameras\n", - "import pickle as pkl\n", - "from bokeh.io import show, output_notebook; output_notebook()\n", - "from happypose.toolbox.visualization.bokeh_plotter import BokehPlotter\n", - "from bokeh.plotting import gridplot\n", - "os.environ['CUDA_VISIBLE_DEVICES'] = '0'\n", - "from pathlib import Path\n", - "import pandas as pd\n", - "from PIL import Image\n", - "import scipy\n", - "\n", - "\n", - "from happypose.pose_estimators.megapose.training.utils import cast_images, cast_to_numpy, CudaTimer\n", - "from happypose.toolbox.lib3d.camera_geometry import get_K_crop_resize\n", - "from happypose.toolbox.datasets.scene_dataset import SceneObservation\n", - "\n", - "from happypose.pose_estimators.megapose.inference.pose_estimator import PoseEstimator, ObservationTensor\n", - "from happypose.pose_estimators.megapose.inference.icp_refiner import ICPRefiner\n", - "from happypose.toolbox.visualization.utils import adjust_brightness, tensor_image_to_uint8, \\\n", - "get_ds_info, make_contour_overlay\n", - "from happypose.toolbox.utils import transform_utils\n", - "from happypose.toolbox.lib3d.cosypose_ops import (\n", - " TCO_init_from_boxes,\n", - " TCO_init_from_boxes_zup_autodepth,\n", - " TCO_init_from_boxes_autodepth_with_R\n", - ")\n", - "from happypose.pose_estimators.megapose.panda3d_renderer.types import Panda3dLightData\n", - "\n", - "\n", - "import matplotlib.pyplot as plt\n", - "%matplotlib inline\n", - "\n", - "BRIGHTNESS_FACTOR=1.5\n", - "\n", - "# zmq_url = \"tcp://127.0.0.1:6000\"\n", - "# zmq_url = \"tcp://127.0.0.1:6001\"\n", - "# zmq_url = \"tcp://127.0.0.1:6004\"" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "58da3c07", - "metadata": {}, - "outputs": [], - "source": [ - "def get_scene_data(scene_ds, scene_id, view_id):\n", - " df = scene_ds.frame_index\n", - " x = df[(df.scene_id == scene_id) & (df.view_id==view_id)]\n", - " ds_idx = x.iloc[0].name\n", - " scene_data = scene_ds[ds_idx]\n", - " return scene_data\n", - "\n", - "\n", - "def orthogonalize_rotation(T):\n", - " rot = scipy.spatial.transform.Rotation.from_matrix(T[:3,:3])\n", - " T[:3,:3] = rot.as_matrix()\n", - " return T" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "48bbcc2a", - "metadata": {}, - "outputs": [], - "source": [ - "object_label = None\n", - "\n", - "ds_name = 'ycbv'\n", - "scene_ds_name = f\"{ds_name}.test\"\n", - "n_refiner_iterations = 5\n", - "\n", - "\n", - "scene_id, im_idx, object_label = 54, 1, 'obj_000015' # drill\n", - "# scene_id, im_idx, object_label = 54, 1, 'obj_000003' # sugargox\n", - "\n", - "\n", - "view_id = im_idx" - ] - }, - { - "cell_type": "markdown", - "id": "b1ce8f1a", - "metadata": {}, - "source": [ - "## Load data and visualize image" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "b5a8829f", - "metadata": {}, - "outputs": [], - "source": [ - "# Load the images/data\n", - "scene_ds_kwargs = {'load_depth': True}\n", - "scene_ds = make_scene_dataset(scene_ds_name, **scene_ds_kwargs)\n", - "scene_data = get_scene_data(scene_ds, scene_id, view_id)\n", - " \n", - "if scene_data.depth is not None:\n", - " depth = torch.as_tensor(scene_data.depth).unsqueeze(-1)\n", - " rgb = torch.as_tensor(scene_data.rgb)\n", - " image = torch.cat([rgb, depth], dim=-1).numpy()\n", - "else:\n", - " image = scene_data.rgb.numpy()\n", - "\n", - "images = [image]\n", - "cameras = make_cameras([scene_data.camera_data])\n", - "\n", - "plotter = BokehPlotter()\n", - "image_f = plotter.plot_image(images[0][...,RGB_DIMS].astype(np.uint8))\n", - "show(image_f)\n", - "\n", - "\n", - "if object_label is None:\n", - " object_labels = None\n", - "else:\n", - " object_labels = [object_label]\n", - "data = SceneObservation.collate_fn([scene_data], object_labels=object_labels)\n", - "observation_tensor = ObservationTensor.from_numpy(scene_data.rgb, depth=scene_data.depth, K=scene_data.camera_data.K)\n", - "observation_tensor = observation_tensor.cuda()\n", - "\n", - "\n", - "# Filter gt_detections to only keep the object we are interested in\n", - "gt_detections = data['gt_detections']\n", - "# Filter and only run the estimator for that object\n", - "df = gt_detections.infos\n", - "df = df[df.label == object_label]\n", - "detection_idx = df.iloc[0].name\n", - "gt_detections = gt_detections[[detection_idx]]\n", - "gt_detections = gt_detections.cuda()" - ] - }, - { - "cell_type": "markdown", - "id": "da56307a", - "metadata": {}, - "source": [ - "## Load the model\n", - "\n", - "Select whether to load a depth refiner or not and what type" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "75cc6509", - "metadata": { - "scrolled": true, - "tags": [] - }, - "outputs": [], - "source": [ - "import happypose.toolbox.inference.utils\n", - "cfg = OmegaConf.create()\n", - "cfg.model_name = 'sn-gso-4views-normals'\n", - "cfg.ds_name = ds_name\n", - "cfg.use_icp = False\n", - "depth_multiplier = None\n", - "per_iter_depth_multiplier = None\n", - "model_data = happypose.toolbox.inference.utils.load_named_model(cfg)\n", - "result_name = cfg.model_name\n", - "\n", - "\n", - "\n", - "refiner_model = model_data['refiner_model']\n", - "coarse_model = model_data['coarse_model']\n", - "obj_ds_name = model_data['obj_ds_name']\n", - "detector_model = model_data['detector_model']\n", - "renderer = refiner_model.renderer\n", - "\n", - "mesh_db = refiner_model.mesh_db\n", - "\n", - "\n", - "depth_refiner = None\n", - "depth_refiner_type = None\n", - "# depth_refiner_type = \"icp\"\n", - "# depth_refiner_type = \"teaser++\"\n", - "\n", - "if depth_refiner_type == \"icp\":\n", - " depth_refiner = ICPRefiner(mesh_db, renderer)\n", - "elif depth_refiner_type == \"teaserpp\":\n", - " from happypose.pose_estimators.megapose.inference.teaserpp_refiner import TeaserppRefiner\n", - " depth_refiner = TeaserppRefiner(mesh_db, renderer)\n", - "\n", - "pose_estimator = PoseEstimator(refiner_model=refiner_model,\n", - " coarse_model=coarse_model,\n", - " detector_model=detector_model,\n", - " depth_refiner=depth_refiner,\n", - " bsz_objects=16,\n", - " bsz_images=576,\n", - " )\n", - "\n" - ] - }, - { - "cell_type": "markdown", - "id": "297bb8d8", - "metadata": {}, - "source": [ - "## Run Model Inference\n", - "\n", - "- We perform the individual steps (detector, coarse, refiner, scoring) etc. separately to make the inference pipeline transparent. You can simply use pose_estimator.run_inference_pipeline to run them all at once.\n", - "- You can set the options as to whether to use the ground-truth detections or the detections from Mask-RCNN.\n", - "- Note: If you aren't using gt_detections and there are multiple object instances in the scene this won't work properly.\n", - "- If you are getting CUDA out of memory errors decrease `bsz_objects` and `bsz_images` to smaller values." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "c3ec0b35", - "metadata": {}, - "outputs": [], - "source": [ - "from happypose.toolbox.utils.tensor_collection import filter_pose_estimates\n", - "\n", - "# Options for inference\n", - "use_gt_detections = True # Note, if you aren't using gt_detections then this should be false\n", - "n_refiner_iterations = 5\n", - "n_pose_hypotheses = 5\n", - "return_debug_data = True\n", - "detection_filter_kwargs = {'labels': [object_label]}\n", - "run_depth_refiner = False\n", - "\n", - "\n", - "bsz_images = 128\n", - "bsz_objects = 2\n", - "\n", - "\n", - "pose_estimator.bsz_objects = bsz_objects\n", - "pose_estimator.bsz_images = bsz_images\n", - "\n", - "# set the random seed\n", - "seed = 0\n", - "torch.manual_seed(seed)\n", - "np.random.seed(seed)\n", - "random.seed(seed)\n", - "\n", - "\n", - "start_time = time.time()\n", - "with torch.no_grad():\n", - " \n", - " if use_gt_detections:\n", - " detections = gt_detections\n", - " else:\n", - " # Only keep the top detection in each image\n", - " detections = pose_estimator.forward_detection_model(observation_tensor, one_instance_per_class=True)\n", - " \n", - " # Filter and only run the estimator for that object\n", - " \n", - " detections = happypose.toolbox.inference.utils.filter_detections(detections, **detection_filter_kwargs)\n", - " detections = happypose.toolbox.inference.utils.add_instance_id_to_detections(detections)\n", - " detections = detections.cuda()\n", - " \n", - "\n", - "# print(\"detections\\n\", detections)\n", - " \n", - " # We have split the inference into it's component steps for clarity. This is a copy of\n", - " # what is in the pose_estimator.run_pipeline method\n", - " # Run the coarse estimator using gt_detections\n", - " data_TCO_coarse, extra_data = pose_estimator.forward_coarse_model(observation=observation_tensor,\n", - " detections=detections, cuda_timer=True)\n", - " \n", - " print(f\"Forward Coarse: total={extra_data['time']:.2f}, \"\\\n", - " f\"model_time={extra_data['model_time']:.2f}, render_time={extra_data['render_time']:.2f}\")\n", - " \n", - " # Extract top-K coarse hypotheses\n", - " data_TCO_filtered = filter_pose_estimates(data_TCO_coarse,\n", - " top_K=n_pose_hypotheses, \n", - " group_cols=[\"batch_im_id\", \"label\", \"instance_id\"], \n", - " filter_field='coarse_logit')\n", - "\n", - " # Refine the top_K coarse hypotheses\n", - " preds, extra_data = pose_estimator.forward_refiner(observation_tensor, data_TCO_filtered, \n", - " n_iterations=n_refiner_iterations, keep_all_outputs=True)\n", - " \n", - " print(f\"Refiner time: {extra_data['time']:.2f}\")\n", - " data_TCO_refined = preds[f'iteration={n_refiner_iterations}']\n", - " refiner_preds = preds\n", - " refiner_outputs = extra_data['outputs']\n", - " \n", - " # Score the refined poses using the coarse model.\n", - " data_TCO_scored, extra_data = pose_estimator.forward_scoring_model(observation_tensor, data_TCO_refined)\n", - "\n", - " # Extract the highest scoring pose estimate for each instance_id\n", - " data_TCO_final = filter_pose_estimates(data_TCO_scored, \n", - " top_K=1, \n", - " group_cols=[\"batch_im_id\", \"label\", \"instance_id\"], \n", - " filter_field='pose_logit')\n", - " \n", - " \n", - " if run_depth_refiner:\n", - " print(\"\\n\\n\")\n", - " t = time.time()\n", - " data_TCO_depth_refiner, _ = pose_estimator.run_depth_refiner(observation_tensor, data_TCO_final,\n", - " )\n", - " depth_refiner_time = time.time() - t\n", - " else:\n", - " data_TCO_depth_refiner = None\n", - " \n", - " \n", - "elapsed = time.time() - start_time\n", - "print(f\"Entire pose estimation pipeline took {elapsed:.2f} seconds\")\n", - "\n", - "print(\"Final Pose Estimate\\n\")\n", - "print(data_TCO_final)\n" - ] - }, - { - "cell_type": "markdown", - "id": "c2061433", - "metadata": {}, - "source": [ - "## Run the entire pipeline\n", - "\n", - "- The cell below shows how to run the entire pipeline in one function call, rather than each step individually.\n", - "- It is disabled by default, set the flag to `True` to run the function." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "664f8ceb", - "metadata": {}, - "outputs": [], - "source": [ - "if False:\n", - " use_gt_detections = False\n", - " if use_gt_detections:\n", - " detections_in = gt_detections.cuda()\n", - " run_detector=False\n", - " else:\n", - " detections_in = None\n", - " run_detector=True\n", - "\n", - "\n", - " detection_filter_kwargs = {'labels': [object_label], 'one_instance_per_class':True}\n", - "\n", - " data_TCO_out, pred_data = pose_estimator.run_inference_pipeline(observation_tensor,\n", - " detections=detections_in,\n", - " run_detector=run_detector,\n", - " n_refiner_iterations=5,\n", - " n_pose_hypotheses=5,\n", - " detection_filter_kwargs=detection_filter_kwargs,\n", - " cuda_timer=True)\n", - " \n", - " print(f\"Inference pipeline: {pred_data['timing_str']}\")\n", - " print(f\"Coarse model: {pred_data['coarse']['data']['timing_str']}\")" - ] - }, - { - "cell_type": "markdown", - "id": "3039fcdc", - "metadata": {}, - "source": [ - "## Extract data from refiner iterations" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "001c05de", - "metadata": { - "tags": [] - }, - "outputs": [], - "source": [ - "\n", - "def plot_cosypose(data, data_TCO_final, refiner_outputs_in, object_label, \n", - " plot_iter=[1, 2, 3, 4], scene_data=None):\n", - "# orig_renderer = object_predictor.pose_predictor.refiner_model.renderer\n", - "# object_predictor.pose_predictor.refiner_model.renderer = renderer\n", - " \n", - " rows = []\n", - " outputs = []\n", - " \n", - " df = data_TCO_final.infos\n", - " df_filter = df[df.label == object_label]\n", - " assert len(df_filter) == 1, f\"There was more than one object named {object_label} in refiner_preds\"\n", - " \n", - " refiner_batch_idx = df_filter.iloc[0]['refiner_batch_idx']\n", - " refiner_instance_idx = df_filter.iloc[0][\"refiner_instance_idx\"]\n", - " \n", - "\n", - " df_gt = data['gt_detections'].infos\n", - " df_gt_filter = df_gt[df_gt.label == object_label]\n", - " \n", - " assert len(df_gt_filter) == 1, f\"There was more than one object named {object_label} in data['gt_detections']\"\n", - " obj_idx_gt = df_gt_filter.iloc[0].name\n", - " TWC = scene_data.camera_data.TWC.matrix\n", - " TCO_gt = data['gt_detections'].poses[obj_idx_gt].cpu().numpy().astype(np.float64)\n", - " TOC_gt = np.linalg.inv(TCO_gt)\n", - " \n", - " \n", - " if 'data_TCO_init' in all_preds:\n", - " data_TCO_init = all_preds['data_TCO_init']\n", - " df = data_TCO_init.infos\n", - " df = df[df.label == object_label]\n", - " idx_tmp = df.index[0]\n", - " TCO_coarse_init = cast_to_numpy(data_TCO_init.poses[idx_tmp], np.float64)\n", - " else:\n", - " TCO_coarse_init = None\n", - " \n", - " \n", - "\n", - " for n in plot_iter:\n", - " refiner_outputs_iter = refiner_outputs_in[refiner_batch_idx][f'iteration={n}']\n", - " image_crop = refiner_outputs[refiner_batch_idx][f'iteration={n}']['images_crop']\\\n", - " [refiner_instance_idx][RGB_DIMS]\n", - " render_crop = refiner_outputs[refiner_batch_idx][f'iteration={n}']['renders']\\\n", - " [refiner_instance_idx][RGB_DIMS]\n", - "\n", - " image_crop = (image_crop.permute(1, 2, 0) * 255).cpu().numpy().astype(np.uint8)\n", - " render_crop = (render_crop.permute(1, 2, 0) * 255).cpu().numpy().astype(np.uint8)\n", - "\n", - " image_f = plotter.plot_image(image_crop)\n", - " render_f = plotter.plot_image(render_crop)\n", - " overlay_f = plotter.plot_overlay(image_crop, render_crop)\n", - " row = [image_f, render_f, overlay_f]\n", - " TCO_pred = refiner_outputs[refiner_batch_idx][f'iteration={n}']['TCO_input'][refiner_instance_idx].cpu().numpy()\n", - " TCO_output = refiner_outputs[refiner_batch_idx][f'iteration={n}']['TCO_input'][refiner_instance_idx].cpu().numpy()\n", - " \n", - " \n", - " # compute errors\n", - " TCO_pred = orthogonalize_rotation(TCO_pred)\n", - " TOgt_O = np.linalg.inv(TCO_gt) @ TCO_pred\n", - " TOgt_O = orthogonalize_rotation(TOgt_O)\n", - " trans_err = np.linalg.norm(TOgt_O[:3,3])\n", - " \n", - " \n", - " # Compute coarse score\n", - " rgb = data['rgb']\n", - " depth = data['depth']\n", - "\n", - " # [B,C,H,W], C=3 or 4 depending on if depth was empty or not\n", - " # Compute score from coarse model\n", - " images = cast_images_to_tensor(rgb, depth)\n", - " K = data['cameras'].K.cuda().float()\n", - " label = [object_label]\n", - " TCO_pred_tensor = torch.tensor(TCO_pred).cuda().unsqueeze(0)\n", - " out_ = coarse_model.forward_coarse(images, K, label, TCO_input=TCO_pred_tensor, \n", - " return_debug_data=True)\n", - " \n", - " coarse_out = out_\n", - " \n", - " \n", - " try:\n", - " _, rot_err_angle_radians = transforms3d.axangles.mat2axangle(TOgt_O[:3,:3])\n", - " rot_err_deg = np.rad2deg(np.abs(rot_err_angle_radians))\n", - " except ValueError:\n", - " print(\"got error while computing angle distance\")\n", - " rot_err_deg = -1\n", - " \n", - "\n", - "\n", - "\n", - " infos = dict(figures=row, \n", - " TCO_output=TCO_output,\n", - " TCO_input=TCO_pred,\n", - " TCO_gt=TCO_gt,\n", - " TOC_gt=TOC_gt,\n", - " TOgt_O=TOgt_O,\n", - " label=object_label,\n", - " refiner_batch_idx=refiner_batch_idx,\n", - " refiner_instance_idx=refiner_instance_idx,\n", - " iteration=n,\n", - " refiner_outputs=refiner_outputs_iter,\n", - " scene_data=scene_data,\n", - " input_rgb_dims=copy.copy(refiner_model.input_rgb_dims),\n", - " input_depth_dims=copy.copy(refiner_model.input_depth_dims),\n", - " render_rgb_dims=copy.copy(refiner_model.render_rgb_dims),\n", - " render_depth_dims=copy.copy(refiner_model.render_depth_dims),\n", - " TCO_coarse_init=TCO_coarse_init,\n", - " trans_err=trans_err,\n", - " rot_err=rot_err_deg,\n", - " coarse_out=coarse_out,\n", - "# TCO_coarse_init=TCO_coarse_init,\n", - "# object_predictor_data=object_predictor_data,\n", - " )\n", - " outputs.append(infos)\n", - " \n", - " return outputs\n", - "\n", - "\n", - "\n", - "\n", - "plot_iter = [1, 2, 3,4,5,6]\n", - "plot_iter = list(range(1, n_refiner_iterations+1))\n", - "# plot_iter = [1,2,3,4,5,6,7,8]\n", - "all_preds = preds\n", - "all_infos = plot_cosypose(data, data_TCO_final, refiner_outputs,\n", - " object_label, plot_iter, scene_data=scene_data)\n", - "\n", - "for info in all_infos:\n", - " info['result_name'] = result_name\n", - "all_infos = pd.DataFrame(all_infos)\n", - "\n", - "# save_path = NB_DATA_DIR / f'{result_name}_ds_name={ds_name}_scene_id={scene_id}_im={view_id}_object_label={object_label}.pkl'\n", - "# save_path.write_bytes(pkl.dumps(all_infos.drop(columns=('figures'))))\n", - "# print(\"wrote\", save_path)" - ] - }, - { - "cell_type": "markdown", - "id": "3b69819c", - "metadata": {}, - "source": [ - "## Make contour overlay figure\n", - "\n", - "Overlay ground-truth and estimated pose" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "19771898", - "metadata": {}, - "outputs": [], - "source": [ - "\n", - "SAVE_DIR = NB_DATA_DIR/'figures'\n", - "SAVE_DIR.mkdir(parents=True, exist_ok=True)\n", - "\n", - "\n", - "ambient_light_data = Panda3dLightData('ambient')\n", - "light_datas = [[ambient_light_data]]\n", - "\n", - "\n", - "# Need to render an image at the ground-truth pose\n", - "d = dict()\n", - "for n in [n_refiner_iterations]:\n", - " \n", - " # Initial coarse estimate\n", - " x = refiner_outputs[0][f'iteration={n}']\n", - " render_img_tensor = x['renders'][0,0:3]\n", - " render_img = tensor_image_to_uint8(render_img_tensor)\n", - " render_img_PIL = Image.fromarray(render_img)\n", - " render_img_PIL = adjust_brightness(render_img_PIL, factor=BRIGHTNESS_FACTOR)\n", - " render_img_PIL.save(f'{SAVE_DIR}/refiner_iter={n}_render.png')\n", - "\n", - "\n", - " img_tensor = x['images_crop'][0, 0:3]\n", - " img = tensor_image_to_uint8(img_tensor)\n", - " img_PIL = Image.fromarray(img)\n", - "\n", - " img_PIL.save(SAVE_DIR/f\"refiner_iter={n}_img_crop.png\")\n", - "\n", - " blend = Plotter.make_overlay(img, np.array(render_img_PIL))\n", - " \n", - " contour_out = make_contour_overlay(img, np.array(render_img_PIL), dilate_iterations=1, color=[0,255,0])\n", - " contour = contour_out['img']\n", - " \n", - " contour_both = make_contour_overlay(img, np.array(render_img_PIL), color=[255,0,0],\n", - " dilate_iterations=0)['img']\n", - "\n", - " \n", - " ### Render image at the ground-truth pose #######\n", - " # [1,3,3]\n", - " pred_idx = 0\n", - " K = x['K_crop'][pred_idx].unsqueeze(0)\n", - " \n", - " df = all_infos\n", - " df = df[df.iteration==n]\n", - "\n", - " # [1,4,4]\n", - " TCO_gt = torch.tensor(df.iloc[0].TCO_gt).unsqueeze(0)\n", - " \n", - " \n", - "# if n > 1:\n", - "# print(\"TCO:\\n\", x['TCO_input'])\n", - "# print(\"TCO_gt:\\n\", TCO_gt)\n", - " \n", - "# TCO_gt = x['TCO_output'][pred_idx].unsqueeze(0)\n", - " obj_infos = [{'name': x['labels'][pred_idx]}]\n", - "# print(\"obj_infos\", obj_infos)\n", - "\n", - " \n", - "\n", - " render_out = renderer.render(labels=[object_label],\n", - " TCO=TCO_gt,\n", - " K=K,\n", - " resolution=img.shape[:2],\n", - " light_datas=light_datas)\n", - " \n", - " \n", - " render_img_gt_tensor = render_out.rgbs[0]\n", - " render_img_gt = tensor_image_to_uint8(render_img_gt_tensor)\n", - "\n", - " render_img_gt_PIL = Image.fromarray(render_img_gt)\n", - " render_img_gt_PIL = adjust_brightness(render_img_gt_PIL, factor=BRIGHTNESS_FACTOR)\n", - " render_img_gt_PIL.save(f'{SAVE_DIR}/refiner_iter={n}_render_gt_pose.png')\n", - " \n", - " \n", - " contour_both = make_contour_overlay(contour_both, np.array(render_img_gt_PIL), color=[0,255,0],\n", - " dilate_iterations=0)['img']\n", - " \n", - " \n", - " \n", - " \n", - " contour_both_PIL = Image.fromarray(contour_both)\n", - " contour_both_PIL.save(f'{SAVE_DIR}/refiner_iter={n}_contour_both.png')\n", - " \n", - " \n", - " if data_TCO_depth_refiner is not None:\n", - " df = data_TCO_depth_refiner.infos\n", - " df = df[df.label == object_label]\n", - " assert len(df) == 1, f\"Found more than one prediction with label {object_label}\"\n", - " TCO = data_TCO_depth_refiner.poses[df.index.tolist()]\n", - " render_out = renderer.render(labels=[object_label],\n", - " TCO=TCO,\n", - " K=K,\n", - " resolution=img.shape[:2],\n", - " light_datas=light_datas)\n", - " render_img_depth_refiner_tensor = render_out.rgbs[0]\n", - " render_img_depth_refiner = tensor_image_to_uint8(render_img_gt_tensor)\n", - " contour_depth_refiner = make_contour_overlay(img, render_img_depth_refiner, dilate_iterations=1, color=[0,255,0])\n", - " \n", - " else:\n", - " contour_depth_refiner = None\n", - " \n", - "\n", - " \n", - " d[n] = {'render': np.array(render_img_PIL),\n", - " 'img': img,\n", - " 'blend': blend,\n", - " 'contour': contour,\n", - " 'contour_out': contour_out,\n", - " 'render_gt': np.array(render_img_gt_PIL),\n", - " 'contour_both': np.array(contour_both_PIL),\n", - " 'contour_depth_refiner': contour_depth_refiner,\n", - " }\n", - " \n", - "\n", - "\n", - "\n", - "plt.figure()\n", - "plt.imshow(d[n_refiner_iterations]['img'])\n", - "plt.show()\n", - "\n", - "plt.figure()\n", - "plt.imshow(d[n_refiner_iterations]['render'])\n", - "plt.show()\n", - "\n", - "plt.figure()\n", - "plt.imshow(d[n_refiner_iterations]['contour_out']['img'])\n", - "plt.title(\"Megapose Refiner\")\n", - "plt.show()\n", - "\n", - "if d[n_refiner_iterations]['contour_depth_refiner'] is not None:\n", - " plt.figure()\n", - " plt.imshow(d[n_refiner_iterations]['contour_depth_refiner']['img'])\n", - " plt.title(\"Megapose + Depth refiner\")\n", - " plt.show()\n", - "\n" - ] - }, - { - "cell_type": "markdown", - "id": "7d367460", - "metadata": {}, - "source": [ - "## Visualize refiner iterations" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "058eb3ae", - "metadata": {}, - "outputs": [], - "source": [ - "print(f\"scene_id: {scene_id}, view_id: {im_idx}, object_label: {object_label}\")\n", - "grid = []\n", - "# plot_iter = [1,2,3,4,5,6,7,8]\n", - "plot_object_label = [object_label]\n", - "df = all_infos.copy()\n", - "df = df.loc[(df['iteration'].isin(plot_iter)) & (df['label'] == object_label)]\n", - "for _, row in df.iterrows():\n", - " figures = row['figures']\n", - " result_name = row['result_name']\n", - " logit = float(row['coarse_out']['logits'][0])\n", - " score = float(row['coarse_out']['scores'][0])\n", - " k = row['iteration']\n", - " figures[1].title.text = f'{result_name} / iter={k} / logit={logit:.1f}, score={score:.1f} '\n", - " figures[2].title.text_font_size = '12pt'\n", - " grid.append(figures)\n", - "show(gridplot(grid, sizing_mode='scale_width'))" - ] - }, - { - "cell_type": "markdown", - "id": "820b9200", - "metadata": {}, - "source": [ - "## 3D visualization using meshcat.\n", - "\n", - "Make sure you have a `meshcat-server` process running on the host machine. Otherwise this code will hang." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "8ccdae7d", - "metadata": {}, - "outputs": [], - "source": [ - "from happypose.toolbox.visualization import meshcat_utils\n", - "from happypose.toolbox.visualization.meshcat_visualizer import MeshcatSceneViewer\n", - "import meshcat.geometry as g\n", - "vis = meshcat_utils.create_visualizer()\n", - "\n", - "df = all_infos\n", - "show_iter = [n_refiner_iterations]\n", - "df = df.loc[(df['iteration'].isin(show_iter))]\n", - "\n", - "viewer = MeshcatSceneViewer(obj_ds_name, use_textures=True,)\n", - "# viewer = MeshcatSceneViewer('ycbv', use_textures=False) # debugging\n", - "vis = viewer.visualizer\n", - "\n", - "def extract_pointcloud_from_scene_data(scene_data):\n", - " depth = scene_data.depth\n", - " K = scene_data.camera_data.K\n", - " pc = meshcat_utils.get_pointcloud(depth, K) \n", - " \n", - " return pc\n", - "\n", - "\n", - "def extract_pointclouds(iter_info):\n", - " refiner_outputs = iter_info['refiner_outputs']\n", - " unique_id = iter_info['refiner_instance_idx']\n", - " image_crop_raw = refiner_outputs['images_crop_raw'][unique_id]\n", - " render_raw = refiner_outputs['renders_raw'][unique_id]\n", - " K_crop = refiner_outputs['K_crop'][unique_id].cpu().numpy()\n", - " KV_crop = refiner_outputs['KV_crop'][unique_id][0].cpu().numpy()\n", - " TCO_input = refiner_outputs['TCO_input'][unique_id].cpu().numpy()\n", - "\n", - " input_depth_dims = iter_info['input_depth_dims']\n", - " render_depth_dims = iter_info['render_depth_dims']\n", - " if len(input_depth_dims) > 0:\n", - " input_depth = image_crop_raw[input_depth_dims].permute(1,2,0).cpu().squeeze().numpy()\n", - " gt_pc = meshcat_utils.get_pointcloud(input_depth, K_crop) \n", - " else:\n", - " gt_pc = None\n", - " \n", - " \n", - " if len(render_depth_dims) > 0:\n", - " render_depth = render_raw[render_depth_dims].permute(1,2,0).cpu().squeeze().numpy()\n", - " render_pc = meshcat_utils.get_pointcloud(render_depth, KV_crop) \n", - " else:\n", - " render_pc = None\n", - " \n", - " \n", - " return {'gt': gt_pc,\n", - " 'render': render_pc,\n", - " 'TCO_input': TCO_input,\n", - " }\n", - " \n", - "def plot_results(df):\n", - " df = df.to_dict('records')\n", - " \n", - " obj_infos = []\n", - " TCO_gt = df[0]['TCO_gt']\n", - " TOC_gt = np.linalg.inv(TCO_gt)\n", - " obj_label = df[0]['label']\n", - " # Visualize camera\n", - " \n", - " for row in df:\n", - " meshcat_prefix = f\"{row['result_name']}\"\n", - " TOgt_O = np.linalg.inv(TCO_gt) @ row['TCO_input']\n", - " k = f\"{row['result_name']}/iteration={row['iteration']}/mesh\"\n", - " obj_infos.append({'name': obj_label, 'TWO': TOgt_O, 'node_name': k})\n", - " \n", - " \n", - " if data_TCO_depth_refiner is not None:\n", - " df_ = data_TCO_depth_refiner.infos\n", - " idx = df_[df_.label == object_label].iloc[0].name\n", - " TCO_depth_refiner = data_TCO_depth_refiner.poses[idx].cpu().numpy()\n", - " TOgt_O = np.linalg.inv(TCO_gt) @ TCO_depth_refiner\n", - " k = f\"{row['result_name']}/depth_refiner/mesh\"\n", - " obj_infos.append({'name': obj_label, 'TWO': TOgt_O, 'node_name': k})\n", - " \n", - " \n", - "# if 'TCO_coarse_init' in df[0]:\n", - "# TCO_coarse_init = cast_to_numpy(df[0]['TCO_coarse_init'], np.float64)\n", - "# TOgt_O = TOC_gt @ TCO_coarse_init\n", - "# obj_infos.append({'name': obj_label, 'TWO': TOgt_O, 'node_name': f'{meshcat_prefix}/TCO_coarse_init/mesh'})\n", - " \n", - " \n", - " obj_infos.append({'name': obj_label, 'TWO': np.eye(4), 'node_name': 'ground_truth'})\n", - " viewer.visualize_scene(obj_infos)\n", - " \n", - " # Extra visualization must be after the 'visualize_scene' call\n", - " meshcat_utils.make_frame(vis, \"camera\", transform=TOC_gt, ignore_invalid_transform=True)\n", - " \n", - " # Line connecting camera and origin\n", - " vertices = np.zeros([3,2])\n", - " vertices[:, 1] = TOC_gt[:3,3]\n", - " vis['line'].set_object(g.Line(g.PointsGeometry(vertices)))\n", - " \n", - " \n", - " # visualize ground-truth pointcloud\n", - " pc_gt = extract_pointcloud_from_scene_data(df[0]['scene_data'])\n", - " meshcat_utils.visualize_pointcloud(vis, 'ground_truth_pointcloud', pc_gt, transform=TOC_gt,\n", - " color=[0,255,0])\n", - " \n", - " \n", - " # visualize depth images, but only for final index\n", - " for row in df:\n", - " pc_data = extract_pointclouds(row)\n", - " f\"{row['result_name']}/iteration={row['iteration']}\"\n", - "# if pc_data['gt'] is not None:\n", - "# print(\"Visualizing ground-truth pointcloud\")\n", - "# meshcat_utils.visualize_pointcloud(vis, 'ground_truth_pointcloud', pc_data['gt'], transform=TOC_gt,\n", - "# color=[0,255,0])\n", - " \n", - " if pc_data['render'] is not None:\n", - " TOgt_O = np.linalg.inv(TCO_gt) @ row['TCO_input']\n", - " TCO_input = pc_data['TCO_input']\n", - " TOC_input = np.linalg.inv(TCO_input)\n", - " TOgt_input = TOgt_O @ TOC_input # transform rendered to observed\n", - " \n", - " k = f\"{row['result_name']}/iteration={row['iteration']}/pointcloud\"\n", - " meshcat_utils.visualize_pointcloud(vis, k, pc_data['render'],\n", - " transform=TOC_gt, color=[255,255,0])\n", - " \n", - " return\n", - "\n", - "plot_results(df)" - ] - }, - { - "cell_type": "markdown", - "id": "5d79c7aa", - "metadata": {}, - "source": [ - "## Print accuracy data" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "8eccf3e9", - "metadata": {}, - "outputs": [], - "source": [ - "df = all_infos\n", - "df_first_iter = df[df.iteration==1]\n", - "trans_err_init = float(df_first_iter.trans_err)\n", - "rot_err_init = float(df_first_iter.rot_err)\n", - "last_iter = df.iteration.max()\n", - "# print(\"last_iter\", last_iter)\n", - "df_final_iter = df[df.iteration==df.iteration.max()]\n", - "\n", - "trans_err = float(df_final_iter.trans_err)\n", - "rot_err = float(df_final_iter.rot_err)\n", - "# print(df_final_iter.tran)\n", - "# df_final_iter = df_final_iter.iloc[0]\n", - "\n", - "SO3_grid_size = pose_estimator._SO3_grid.shape[0]\n", - "# print(x_failure)\n", - "print(f\"result_name: {result_name}, SO3-grid-size={SO3_grid_size}\")\n", - "print(f\"iteration={last_iter}\")\n", - "print(f\"per_iter_depth_multiplier:\", refiner_model.per_iter_depth_multiplier)\n", - "print(\"depth_multiplier:\", refiner_model.depth_multiplier)\n", - "print(f\"\\ninitial translation error (cm): {trans_err_init*100:.2f}\")\n", - "print(f\"initial rot_err (no sym) (deg): {rot_err_init:.1f}\")\n", - "print(f\"\\ntranslation error (cm): {trans_err*100:.2f}\\nrot_err (no sym) (deg): {rot_err:.1f}\")" - ] - }, - { - "cell_type": "markdown", - "id": "9f7f8e12", - "metadata": {}, - "source": [ - "##### \n", - "\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "\n", - "\n" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "7ab857fc", - "metadata": {}, - "outputs": [], - "source": [] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "b9e22c6f", - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3 (ipykernel)", - "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.9.12" - } - }, - "nbformat": 4, - "nbformat_minor": 5 + "cells": [ + { + "cell_type": "code", + "execution_count": null, + "id": "d3874bde-0969-418e-9e3d-4cf9ab930413", + "metadata": {}, + "outputs": [], + "source": [ + "%load_ext autoreload\n", + "%autoreload 2\n", + "\n", + "import os\n", + "import copy\n", + "import time\n", + "import torch\n", + "import numpy as np\n", + "import random\n", + "import pickle as pkl\n", + "import transforms3d\n", + "from pathlib import Path\n", + "import pandas as pd\n", + "from PIL import Image\n", + "import scipy\n", + "from bokeh.io import show, output_notebook; output_notebook()\n", + "from bokeh.plotting import gridplot\n", + "from omegaconf import OmegaConf\n", + "import matplotlib.pyplot as plt\n", + "\n", + "# os.environ['HAPPYPOSE_DATA_DIR'] = '/path/to/your/local/dir'\n", + "# os.environ['CUDA_VISIBLE_DEVICES'] = '0'\n", + "\n", + "# Happypose toolbox\n", + "from happypose.toolbox.datasets.datasets_cfg import make_scene_dataset, make_object_dataset\n", + "from happypose.toolbox.datasets.scene_dataset import SceneObservation\n", + "from happypose.toolbox.inference.utils import load_detector, make_cameras, add_instance_id, filter_detections\n", + "from happypose.toolbox.utils import transform_utils\n", + "from happypose.toolbox.lib3d.camera_geometry import get_K_crop_resize\n", + "from happypose.toolbox.lib3d.cosypose_ops import (\n", + " TCO_init_from_boxes,\n", + " TCO_init_from_boxes_zup_autodepth,\n", + " TCO_init_from_boxes_autodepth_with_R\n", + ")\n", + "from happypose.toolbox.renderer.types import Panda3dLightData\n", + "from happypose.toolbox.visualization.bokeh_plotter import BokehPlotter\n", + "from happypose.toolbox.visualization.utils import adjust_brightness, tensor_image_to_uint8, \\\n", + "get_ds_info, make_contour_overlay\n", + "from happypose.toolbox.utils.load_model import load_named_model\n", + "from happypose.toolbox.utils.tensor_collection import filter_top_pose_estimates\n", + "\n", + "\n", + "# import happypose.pose_estimators.megapose\n", + "# Megapose\n", + "from happypose.pose_estimators.megapose.config import LOCAL_DATA_DIR, NB_DATA_DIR\n", + "from happypose.pose_estimators.megapose.training.utils import RGB_DIMS\n", + "from happypose.pose_estimators.megapose.training.utils import cast_images, cast_to_numpy, CudaTimer\n", + "from happypose.pose_estimators.megapose.inference.pose_estimator import PoseEstimator, ObservationTensor\n", + "from happypose.pose_estimators.megapose.inference.icp_refiner import ICPRefiner\n", + "from happypose.pose_estimators.megapose.bop_config import PBR_DETECTORS\n", + "\n", + "%matplotlib inline\n", + "\n", + "BRIGHTNESS_FACTOR=1.5\n", + "\n", + "device = torch.device(\"cuda\" if torch.cuda.is_available() else \"cpu\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "58da3c07", + "metadata": {}, + "outputs": [], + "source": [ + "def get_scene_data(scene_ds, scene_id, view_id):\n", + " df = scene_ds.frame_index\n", + " x = df[(df.scene_id == scene_id) & (df.view_id==view_id)]\n", + " ds_idx = x.iloc[0].name\n", + " scene_data = scene_ds[ds_idx]\n", + " return scene_data\n", + "\n", + "\n", + "def orthogonalize_rotation(T):\n", + " rot = scipy.spatial.transform.Rotation.from_matrix(T[:3,:3])\n", + " T[:3,:3] = rot.as_matrix()\n", + " return T" + ] + }, + { + "cell_type": "markdown", + "id": "c5611ae9", + "metadata": {}, + "source": [ + "## Settings: model name and image data" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "48bbcc2a", + "metadata": {}, + "outputs": [], + "source": [ + "megapose_model_name = 'megapose-1.0-RGB'\n", + "result_name = megapose_model_name\n", + "\n", + "ds_name = 'ycbv'\n", + "scene_ds_name = f\"{ds_name}.test\"\n", + "n_refiner_iterations = 5\n", + "\n", + "detector_run_id = PBR_DETECTORS[ds_name]\n", + "\n", + "scene_id, im_idx, object_label = 54, 1, 'ycbv-obj_000015' # drill\n", + "# scene_id, im_idx, object_label = 54, 1, 'ycbv-obj_000003' # sugargox\n", + "\n", + "view_id = im_idx" + ] + }, + { + "cell_type": "markdown", + "id": "b1ce8f1a", + "metadata": {}, + "source": [ + "## Load data and visualize image" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "b5a8829f", + "metadata": {}, + "outputs": [], + "source": [ + "# Load the images/data\n", + "scene_ds_kwargs = {'load_depth': True}\n", + "scene_ds = make_scene_dataset(scene_ds_name, **scene_ds_kwargs)\n", + "scene_data = get_scene_data(scene_ds, scene_id, view_id)\n", + " \n", + "if scene_data.depth is not None:\n", + " depth = torch.as_tensor(scene_data.depth).unsqueeze(-1)\n", + " rgb = torch.as_tensor(scene_data.rgb)\n", + " image = torch.cat([rgb, depth], dim=-1).numpy()\n", + "else:\n", + " image = scene_data.rgb.numpy()\n", + "\n", + "images = [image]\n", + "cameras = make_cameras([scene_data.camera_data])\n", + "\n", + "plotter = BokehPlotter()\n", + "image_f = plotter.plot_image(images[0][...,RGB_DIMS].astype(np.uint8))\n", + "show(image_f)\n", + "\n", + "\n", + "if object_label is None:\n", + " object_labels = None\n", + "else:\n", + " object_labels = [object_label]\n", + "data = SceneObservation.collate_fn([scene_data], object_labels=object_labels)\n", + "observation_tensor = ObservationTensor.from_numpy(scene_data.rgb, depth=scene_data.depth, K=scene_data.camera_data.K)\n", + "# observation_tensor = observation_tensor.cuda()\n", + "\n", + "\n", + "# Filter gt_detections to only keep the object we are interested in\n", + "gt_detections = data['gt_detections']\n", + "# Filter and only run the estimator for that object\n", + "df = gt_detections.infos\n", + "df = df[df.label == object_label]\n", + "detection_idx = df.iloc[0].name\n", + "gt_detections = gt_detections[[detection_idx]]\n", + "# gt_detections = gt_detections.cuda()" + ] + }, + { + "cell_type": "markdown", + "id": "da56307a", + "metadata": {}, + "source": [ + "## Load the model\n", + "\n", + "Select whether to load a depth refiner or not and what type" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "75cc6509", + "metadata": { + "scrolled": false, + "tags": [] + }, + "outputs": [], + "source": [ + "object_ds = make_object_dataset(ds_name)\n", + "model_data = load_named_model(megapose_model_name, object_ds)\n", + "detector_model = load_detector(detector_run_id)\n", + "\n", + "refiner_model = model_data.refiner_model\n", + "coarse_model = model_data.coarse_model\n", + "renderer = refiner_model.renderer\n", + "\n", + "depth_refiner = None\n", + "depth_refiner_type = None\n", + "# depth_refiner_type = \"icp\"\n", + "# depth_refiner_type = \"teaser++\"\n", + "\n", + "if depth_refiner_type == \"icp\":\n", + " depth_refiner = ICPRefiner(refiner_model.mesh_db, renderer)\n", + "elif depth_refiner_type == \"teaserpp\":\n", + " from happypose.pose_estimators.megapose.inference.teaserpp_refiner import TeaserppRefiner\n", + " depth_refiner = TeaserppRefiner(refiner_model.mesh_db, renderer)\n", + " \n", + "pose_estimator = PoseEstimator(\n", + " refiner_model=refiner_model,\n", + " coarse_model=coarse_model,\n", + " detector_model=detector_model,\n", + " depth_refiner=depth_refiner,\n", + ")\n", + "\n", + "# Run options\n", + "use_gt_detections = True # Note, if you aren't using gt_detections then this should be false\n", + "n_refiner_iterations = 5\n", + "n_pose_hypotheses = 1\n", + "run_depth_refiner = False" + ] + }, + { + "cell_type": "markdown", + "id": "297bb8d8", + "metadata": {}, + "source": [ + "## Run Model Inference\n", + "\n", + "- We perform the individual steps (detector, coarse, refiner, scoring) etc. separately to make the inference pipeline transparent. You can simply use pose_estimator.run_inference_pipeline to run them all at once.\n", + "- You can set the options as to whether to use the ground-truth detections or the detections from Mask-RCNN.\n", + "- Note: If you aren't using gt_detections and there are multiple object instances in the scene this won't work properly.\n", + "- If you are getting CUDA out of memory errors decrease `bsz_objects` and `bsz_images` to smaller values." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "c3ec0b35", + "metadata": {}, + "outputs": [], + "source": [ + "# Options for inference\n", + "bsz_images = 128\n", + "bsz_objects = 2\n", + "\n", + "pose_estimator.bsz_objects = bsz_objects\n", + "pose_estimator.bsz_images = bsz_images\n", + "\n", + "# set the random seed\n", + "seed = 0\n", + "torch.manual_seed(seed)\n", + "np.random.seed(seed)\n", + "random.seed(seed)\n", + "\n", + "\n", + "start_time = time.time()\n", + "with torch.no_grad():\n", + " \n", + " if use_gt_detections:\n", + " detections = gt_detections\n", + " else:\n", + " # Only keep the top detections in each image\n", + " detections = pose_estimator.forward_detection_model(observation_tensor, one_instance_per_class=True)\n", + " \n", + " # Filter and only run the estimator for that object\n", + "# detections = filter_detections(detections, labels=object_labels)\n", + " detections = add_instance_id(detections)\n", + "# detections = detections.cuda()\n", + " \n", + "\n", + " # We have split the inference into it's component steps for clarity. This is a copy of\n", + " # what is in the pose_estimator.run_pipeline method\n", + " # Run the coarse estimator using detections\n", + " data_TCO_coarse, extra_data = pose_estimator.forward_coarse_model(observation=observation_tensor,\n", + " detections=detections, cuda_timer=True)\n", + " \n", + " print(f\"Forward Coarse: total={extra_data['time']:.2f}, \"\\\n", + " f\"model_time={extra_data['model_time']:.2f}, render_time={extra_data['render_time']:.2f}\")\n", + " \n", + " # Extract top-K coarse hypotheses\n", + " data_TCO_filtered = filter_top_pose_estimates(data_TCO_coarse,\n", + " top_K=n_pose_hypotheses, \n", + " group_cols=[\"batch_im_id\", \"label\", \"instance_id\"], \n", + " filter_field='coarse_logit')\n", + "\n", + " # Refine the top_K coarse hypotheses\n", + " preds, extra_data = pose_estimator.forward_refiner(observation_tensor, data_TCO_filtered, \n", + " n_iterations=n_refiner_iterations, keep_all_outputs=True)\n", + " \n", + " print(f\"Refiner time: {extra_data['time']:.2f}\")\n", + " data_TCO_refined = preds[f'iteration={n_refiner_iterations}']\n", + " refiner_preds = preds\n", + " refiner_outputs = extra_data['outputs']\n", + " \n", + " # Score the refined poses using the coarse model.\n", + " data_TCO_scored, extra_data = pose_estimator.forward_scoring_model(observation_tensor, data_TCO_refined)\n", + "\n", + " # Extract the highest scoring pose estimate for each instance_id\n", + " data_TCO_final = filter_top_pose_estimates(data_TCO_scored, \n", + " top_K=1, \n", + " group_cols=[\"batch_im_id\", \"label\", \"instance_id\"], \n", + " filter_field='pose_logit')\n", + " \n", + " \n", + " if run_depth_refiner:\n", + " print(\"\\n\\n\")\n", + " t = time.time()\n", + " data_TCO_depth_refiner, _ = pose_estimator.run_depth_refiner(observation_tensor, data_TCO_final,\n", + " )\n", + " depth_refiner_time = time.time() - t\n", + " else:\n", + " data_TCO_depth_refiner = None\n", + " \n", + " \n", + "elapsed = time.time() - start_time\n", + "print(f\"Entire pose estimation pipeline took {elapsed:.2f} seconds\")\n", + "\n", + "print(\"Final Pose Estimate\\n\")\n", + "print(data_TCO_final)" + ] + }, + { + "cell_type": "markdown", + "id": "c2061433", + "metadata": {}, + "source": [ + "## Run the entire pipeline\n", + "\n", + "- The cell below shows how to run the entire pipeline in one function call, rather than each step individually.\n", + "- It is disabled by default, set the flag to `True` to run the function." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "664f8ceb", + "metadata": {}, + "outputs": [], + "source": [ + "if True:\n", + " use_gt_detections = False\n", + " if use_gt_detections:\n", + " detections_in = gt_detections\n", + " run_detector=False\n", + " else:\n", + " detections_in = None\n", + " run_detector=True\n", + "\n", + "\n", + " detection_filter_kwargs = {'labels': [object_label], 'one_instance_per_class':True}\n", + "\n", + " data_TCO_out, pred_data = pose_estimator.run_inference_pipeline(observation_tensor,\n", + " detections=detections_in,\n", + " run_detector=run_detector,\n", + " n_refiner_iterations=5,\n", + " n_pose_hypotheses=5,\n", + " detection_filter_kwargs=detection_filter_kwargs,\n", + " cuda_timer=True)\n", + " \n", + " print(f\"Inference pipeline: {pred_data['timing_str']}\")\n", + " print(f\"Coarse model: {pred_data['coarse']['data']['timing_str']}\")" + ] + }, + { + "cell_type": "markdown", + "id": "3039fcdc", + "metadata": {}, + "source": [ + "## Extract data from refiner iterations" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "001c05de", + "metadata": { + "tags": [] + }, + "outputs": [], + "source": [ + "def plot_coarse_refiner_iterations(data, data_TCO_final, refiner_outputs_in, object_label, \n", + " plot_iter=[1, 2, 3, 4], scene_data=None):\n", + " rows = []\n", + " outputs = []\n", + " \n", + " df = data_TCO_final.infos\n", + " df_filter = df[df.label == object_label]\n", + " assert len(df_filter) == 1, f\"There was more than one object named {object_label} in refiner_preds\"\n", + " \n", + " refiner_batch_idx = df_filter.iloc[0]['refiner_batch_idx']\n", + " refiner_instance_idx = df_filter.iloc[0][\"refiner_instance_idx\"]\n", + " \n", + "\n", + " df_gt = data['gt_detections'].infos\n", + " df_gt_filter = df_gt[df_gt.label == object_label]\n", + " \n", + " assert len(df_gt_filter) == 1, f\"There was more than one object named {object_label} in data['gt_detections']\"\n", + " obj_idx_gt = df_gt_filter.iloc[0].name\n", + " TWC = scene_data.camera_data.TWC.matrix\n", + " TCO_gt = data['gt_detections'].poses[obj_idx_gt].cpu().numpy().astype(np.float64)\n", + " TOC_gt = np.linalg.inv(TCO_gt)\n", + " \n", + " \n", + " if 'data_TCO_init' in all_preds:\n", + " data_TCO_init = all_preds['data_TCO_init']\n", + " df = data_TCO_init.infos\n", + " df = df[df.label == object_label]\n", + " idx_tmp = df.index[0]\n", + " TCO_coarse_init = cast_to_numpy(data_TCO_init.poses[idx_tmp], np.float64)\n", + " else:\n", + " TCO_coarse_init = None\n", + " \n", + " \n", + "\n", + " for n in plot_iter:\n", + " refiner_outputs_iter = refiner_outputs_in[refiner_batch_idx][f'iteration={n}']\n", + " image_crop = refiner_outputs[refiner_batch_idx][f'iteration={n}'].images_crop\\\n", + " [refiner_instance_idx][RGB_DIMS]\n", + " render_crop = refiner_outputs[refiner_batch_idx][f'iteration={n}'].renders\\\n", + " [refiner_instance_idx][RGB_DIMS]\n", + "\n", + " image_crop = (image_crop.permute(1, 2, 0) * 255).cpu().numpy().astype(np.uint8)\n", + " render_crop = (render_crop.permute(1, 2, 0) * 255).cpu().numpy().astype(np.uint8)\n", + "\n", + " image_f = plotter.plot_image(image_crop)\n", + " render_f = plotter.plot_image(render_crop)\n", + " overlay_f = plotter.plot_overlay(image_crop, render_crop)\n", + " row = [image_f, render_f, overlay_f]\n", + " TCO_pred = refiner_outputs[refiner_batch_idx][f'iteration={n}'].TCO_input[refiner_instance_idx].cpu().numpy()\n", + " TCO_output = refiner_outputs[refiner_batch_idx][f'iteration={n}'].TCO_input[refiner_instance_idx].cpu().numpy()\n", + " \n", + " \n", + " # compute errors\n", + " TCO_pred = orthogonalize_rotation(TCO_pred)\n", + " TOgt_O = np.linalg.inv(TCO_gt) @ TCO_pred\n", + " TOgt_O = orthogonalize_rotation(TOgt_O)\n", + " trans_err = np.linalg.norm(TOgt_O[:3,3])\n", + " \n", + " \n", + " # Compute coarse score\n", + " rgb = data['rgb']\n", + " depth = data['depth']\n", + "\n", + " # [B,C,H,W], C=3 or 4 depending on if depth was empty or not\n", + " # Compute score from coarse model\n", + "# images = cast_images_to_tensor(rgb, depth)\n", + " images = torch.cat((rgb, depth), dim=1)\n", + " K = data['cameras'].K.to(device).float()\n", + " label = [object_label]\n", + " TCO_pred_tensor = torch.tensor(TCO_pred).to(device).unsqueeze(0)\n", + " out_ = coarse_model.forward_coarse(images, K, label, TCO_input=TCO_pred_tensor, \n", + " return_debug_data=True)\n", + " \n", + " coarse_out = out_\n", + " \n", + " try:\n", + " _, rot_err_angle_radians = transforms3d.axangles.mat2axangle(TOgt_O[:3,:3])\n", + " rot_err_deg = np.rad2deg(np.abs(rot_err_angle_radians))\n", + " except ValueError:\n", + " print(\"got error while computing angle distance\")\n", + " rot_err_deg = -1\n", + " \n", + " infos = dict(figures=row, \n", + " TCO_output=TCO_output,\n", + " TCO_input=TCO_pred,\n", + " TCO_gt=TCO_gt,\n", + " TOC_gt=TOC_gt,\n", + " TOgt_O=TOgt_O,\n", + " label=object_label,\n", + " refiner_batch_idx=refiner_batch_idx,\n", + " refiner_instance_idx=refiner_instance_idx,\n", + " iteration=n,\n", + " refiner_outputs=refiner_outputs_iter,\n", + " scene_data=scene_data,\n", + " input_rgb_dims=copy.copy(refiner_model.input_rgb_dims),\n", + " input_depth_dims=copy.copy(refiner_model.input_depth_dims),\n", + " render_rgb_dims=copy.copy(refiner_model.render_rgb_dims),\n", + " render_depth_dims=copy.copy(refiner_model.render_depth_dims),\n", + " TCO_coarse_init=TCO_coarse_init,\n", + " trans_err=trans_err,\n", + " rot_err=rot_err_deg,\n", + " coarse_out=coarse_out,\n", + " )\n", + " outputs.append(infos)\n", + " \n", + " return outputs\n", + "\n", + "\n", + "\n", + "\n", + "plot_iter = [1, 2, 3,4,5,6]\n", + "plot_iter = list(range(1, n_refiner_iterations+1))\n", + "# plot_iter = [1,2,3,4,5,6,7,8]\n", + "all_preds = preds\n", + "all_infos = plot_coarse_refiner_iterations(data, data_TCO_final, refiner_outputs,\n", + " object_label, plot_iter, scene_data=scene_data)\n", + "\n", + "for info in all_infos:\n", + " info['result_name'] = result_name\n", + "all_infos = pd.DataFrame(all_infos)\n", + "\n", + "# save_path = NB_DATA_DIR / f'{result_name}_ds_name={ds_name}_scene_id={scene_id}_im={view_id}_object_label={object_label}.pkl'\n", + "# save_path.write_bytes(pkl.dumps(all_infos.drop(columns=('figures'))))\n", + "# print(\"wrote\", save_path)" + ] + }, + { + "cell_type": "markdown", + "id": "3b69819c", + "metadata": {}, + "source": [ + "## Make contour overlay figure\n", + "\n", + "Overlay ground-truth and estimated pose" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "19771898", + "metadata": {}, + "outputs": [], + "source": [ + "\n", + "SAVE_DIR = NB_DATA_DIR/'figures'\n", + "SAVE_DIR.mkdir(parents=True, exist_ok=True)\n", + "\n", + "\n", + "ambient_light_data = Panda3dLightData('ambient')\n", + "light_datas = [[ambient_light_data]]\n", + "\n", + "\n", + "# Need to render an image at the ground-truth pose\n", + "d = dict()\n", + "for n in [n_refiner_iterations]:\n", + " \n", + " # Initial coarse estimate\n", + " x = refiner_outputs[0][f'iteration={n}']\n", + " render_img_tensor = x.renders[0,0:3]\n", + " render_img = tensor_image_to_uint8(render_img_tensor)\n", + " render_img_PIL = Image.fromarray(render_img)\n", + " render_img_PIL = adjust_brightness(render_img_PIL, factor=BRIGHTNESS_FACTOR)\n", + " render_img_PIL.save(f'{SAVE_DIR}/refiner_iter={n}_render.png')\n", + "\n", + "\n", + " img_tensor = x.images_crop[0, 0:3]\n", + " img = tensor_image_to_uint8(img_tensor)\n", + " img_PIL = Image.fromarray(img)\n", + "\n", + " img_PIL.save(SAVE_DIR/f\"refiner_iter={n}_img_crop.png\")\n", + "\n", + " blend = plotter.plot_overlay(img, np.array(render_img_PIL))\n", + " \n", + " contour_out = make_contour_overlay(img, np.array(render_img_PIL), dilate_iterations=1, color=[0,255,0])\n", + " contour = contour_out['img']\n", + " \n", + " contour_both = make_contour_overlay(img, np.array(render_img_PIL), color=[255,0,0],\n", + " dilate_iterations=0)['img']\n", + "\n", + " \n", + " ### Render image at the ground-truth pose #######\n", + " # [1,3,3]\n", + " pred_idx = 0\n", + " K = x.K_crop[pred_idx].unsqueeze(0)\n", + " \n", + " df = all_infos\n", + " df = df[df.iteration==n]\n", + "\n", + " # [1,4,4]\n", + " TCO_gt = torch.tensor(df.iloc[0].TCO_gt).unsqueeze(0)\n", + " \n", + " obj_infos = [{'name': x.labels[pred_idx]}]\n", + "\n", + " render_out = renderer.render(labels=[object_label],\n", + " TCO=TCO_gt,\n", + " K=K,\n", + " resolution=img.shape[:2],\n", + " light_datas=light_datas)\n", + " \n", + " \n", + " render_img_gt_tensor = render_out.rgbs[0]\n", + " render_img_gt = tensor_image_to_uint8(render_img_gt_tensor)\n", + "\n", + " render_img_gt_PIL = Image.fromarray(render_img_gt)\n", + " render_img_gt_PIL = adjust_brightness(render_img_gt_PIL, factor=BRIGHTNESS_FACTOR)\n", + " render_img_gt_PIL.save(f'{SAVE_DIR}/refiner_iter={n}_render_gt_pose.png')\n", + " \n", + " \n", + " contour_both = make_contour_overlay(contour_both, np.array(render_img_gt_PIL), color=[0,255,0],\n", + " dilate_iterations=0)['img']\n", + " \n", + " \n", + " \n", + " \n", + " contour_both_PIL = Image.fromarray(contour_both)\n", + " contour_both_PIL.save(f'{SAVE_DIR}/refiner_iter={n}_contour_both.png')\n", + " \n", + " \n", + " if data_TCO_depth_refiner is not None:\n", + " df = data_TCO_depth_refiner.infos\n", + " df = df[df.label == object_label]\n", + " assert len(df) == 1, f\"Found more than one prediction with label {object_label}\"\n", + " TCO = data_TCO_depth_refiner.poses[df.index.tolist()]\n", + " render_out = renderer.render(labels=[object_label],\n", + " TCO=TCO,\n", + " K=K,\n", + " resolution=img.shape[:2],\n", + " light_datas=light_datas)\n", + " render_img_depth_refiner_tensor = render_out.rgbs[0]\n", + " render_img_depth_refiner = tensor_image_to_uint8(render_img_gt_tensor)\n", + " contour_depth_refiner = make_contour_overlay(img, render_img_depth_refiner, dilate_iterations=1, color=[0,255,0])\n", + " \n", + " else:\n", + " contour_depth_refiner = None\n", + " \n", + "\n", + " \n", + " d[n] = {'render': np.array(render_img_PIL),\n", + " 'img': img,\n", + " 'blend': blend,\n", + " 'contour': contour,\n", + " 'contour_out': contour_out,\n", + " 'render_gt': np.array(render_img_gt_PIL),\n", + " 'contour_both': np.array(contour_both_PIL),\n", + " 'contour_depth_refiner': contour_depth_refiner,\n", + " }\n", + " \n", + "\n", + "\n", + "\n", + "plt.figure()\n", + "plt.imshow(d[n_refiner_iterations]['img'])\n", + "plt.show()\n", + "\n", + "plt.figure()\n", + "plt.imshow(d[n_refiner_iterations]['render'])\n", + "plt.show()\n", + "\n", + "plt.figure()\n", + "plt.imshow(d[n_refiner_iterations]['contour_out']['img'])\n", + "plt.title(\"Megapose Refiner\")\n", + "plt.show()\n", + "\n", + "if d[n_refiner_iterations]['contour_depth_refiner'] is not None:\n", + " plt.figure()\n", + " plt.imshow(d[n_refiner_iterations]['contour_depth_refiner']['img'])\n", + " plt.title(\"Megapose + Depth refiner\")\n", + " plt.show()\n", + "\n" + ] + }, + { + "cell_type": "markdown", + "id": "7d367460", + "metadata": {}, + "source": [ + "## Visualize refiner iterations" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "058eb3ae", + "metadata": {}, + "outputs": [], + "source": [ + "print(f\"scene_id: {scene_id}, view_id: {im_idx}, object_label: {object_label}\")\n", + "grid = []\n", + "# plot_iter = [1,2,3,4,5,6,7,8]\n", + "plot_object_label = [object_label]\n", + "df = all_infos.copy()\n", + "df = df.loc[(df['iteration'].isin(plot_iter)) & (df['label'] == object_label)]\n", + "for _, row in df.iterrows():\n", + " figures = row['figures']\n", + " result_name = row['result_name']\n", + " logit = float(row['coarse_out']['logits'][0])\n", + " score = float(row['coarse_out']['scores'][0])\n", + " k = row['iteration']\n", + " figures[1].title.text = f'{result_name} / iter={k} / logit={logit:.1f}, score={score:.1f} '\n", + " figures[2].title.text_font_size = '12pt'\n", + " grid.append(figures)\n", + "show(gridplot(grid, sizing_mode='scale_width'))" + ] + }, + { + "cell_type": "markdown", + "id": "820b9200", + "metadata": {}, + "source": [ + "## 3D visualization using meshcat.\n", + "\n", + "Make sure you have a `meshcat-server` process running on the host machine. Otherwise this code will hang." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "8ccdae7d", + "metadata": {}, + "outputs": [], + "source": [ + "from happypose.toolbox.visualization import meshcat_utils\n", + "from happypose.toolbox.visualization.meshcat_visualizer import MeshcatSceneViewer\n", + "import meshcat.geometry as g\n", + "vis = meshcat_utils.create_visualizer()\n", + "\n", + "df = all_infos\n", + "show_iter = [n_refiner_iterations]\n", + "df = df.loc[(df['iteration'].isin(show_iter))]\n", + "\n", + "viewer = MeshcatSceneViewer(ds_name, use_textures=True,)\n", + "vis = viewer.visualizer\n", + "\n", + "def extract_pointcloud_from_scene_data(scene_data):\n", + " depth = scene_data.depth\n", + " K = scene_data.camera_data.K\n", + " pc = meshcat_utils.get_pointcloud(depth, K) \n", + " \n", + " return pc\n", + "\n", + "\n", + "def extract_pointclouds(iter_info):\n", + " refiner_outputs = iter_info['refiner_outputs']\n", + " unique_id = iter_info['refiner_instance_idx']\n", + "# images_crop = refiner_outputs.images_crop[unique_id]\n", + " render_raw = refiner_outputs.renders[unique_id]\n", + " K_crop = refiner_outputs.K_crop[unique_id].cpu().numpy()\n", + " KV_crop = refiner_outputs.KV_crop[unique_id][0].cpu().numpy()\n", + " TCO_input = refiner_outputs.TCO_input[unique_id].cpu().numpy()\n", + "\n", + " input_depth_dims = iter_info['input_depth_dims']\n", + " render_depth_dims = iter_info['render_depth_dims']\n", + " if len(input_depth_dims) > 0:\n", + " input_depth = image_crop_raw[input_depth_dims].permute(1,2,0).cpu().squeeze().numpy()\n", + " gt_pc = meshcat_utils.get_pointcloud(input_depth, K_crop) \n", + " else:\n", + " gt_pc = None\n", + " \n", + " if len(render_depth_dims) > 0:\n", + " render_depth = render_raw[render_depth_dims].permute(1,2,0).cpu().squeeze().numpy()\n", + " render_pc = meshcat_utils.get_pointcloud(render_depth, KV_crop) \n", + " else:\n", + " render_pc = None\n", + " \n", + " return {'gt': gt_pc,\n", + " 'render': render_pc,\n", + " 'TCO_input': TCO_input,\n", + " }\n", + "\n", + "\n", + "def plot_results(df):\n", + " df = df.to_dict('records')\n", + " \n", + " obj_infos = []\n", + " TCO_gt = df[0]['TCO_gt']\n", + " TOC_gt = np.linalg.inv(TCO_gt)\n", + " obj_label = df[0]['label']\n", + " \n", + " # Visualize camera\n", + " for row in df:\n", + " meshcat_prefix = f\"{row['result_name']}\"\n", + " TOgt_O = np.linalg.inv(TCO_gt) @ row['TCO_input']\n", + " k = f\"{row['result_name']}/iteration={row['iteration']}/mesh\"\n", + " obj_infos.append({'name': obj_label, 'TWO': TOgt_O, 'node_name': k})\n", + " \n", + " \n", + " if data_TCO_depth_refiner is not None:\n", + " df_ = data_TCO_depth_refiner.infos\n", + " idx = df_[df_.label == object_label].iloc[0].name\n", + " TCO_depth_refiner = data_TCO_depth_refiner.poses[idx].cpu().numpy()\n", + " TOgt_O = np.linalg.inv(TCO_gt) @ TCO_depth_refiner\n", + " k = f\"{row['result_name']}/depth_refiner/mesh\"\n", + " obj_infos.append({'name': obj_label, 'TWO': TOgt_O, 'node_name': k})\n", + " \n", + " obj_infos.append({'name': obj_label, 'TWO': np.eye(4), 'node_name': 'ground_truth'})\n", + " viewer.visualize_scene(obj_infos)\n", + " \n", + " # Extra visualization must be after the 'visualize_scene' call\n", + " meshcat_utils.make_frame(vis, \"camera\", transform=TOC_gt, ignore_invalid_transform=True)\n", + " \n", + " # Line connecting camera and origin\n", + " vertices = np.zeros([3,2])\n", + " vertices[:, 1] = TOC_gt[:3,3]\n", + " vis['line'].set_object(g.Line(g.PointsGeometry(vertices)))\n", + " \n", + " # visualize ground-truth pointcloud\n", + " pc_gt = extract_pointcloud_from_scene_data(df[0]['scene_data'])\n", + " meshcat_utils.visualize_pointcloud(vis, 'ground_truth_pointcloud', pc_gt, transform=TOC_gt,\n", + " color=[0,255,0])\n", + " \n", + " # visualize depth images, but only for final index\n", + " for row in df:\n", + " pc_data = extract_pointclouds(row)\n", + " f\"{row['result_name']}/iteration={row['iteration']}\"\n", + " \n", + " if pc_data['render'] is not None:\n", + " TOgt_O = np.linalg.inv(TCO_gt) @ row['TCO_input']\n", + " TCO_input = pc_data['TCO_input']\n", + " TOC_input = np.linalg.inv(TCO_input)\n", + " TOgt_input = TOgt_O @ TOC_input # transform rendered to observed\n", + " \n", + " k = f\"{row['result_name']}/iteration={row['iteration']}/pointcloud\"\n", + " meshcat_utils.visualize_pointcloud(vis, k, pc_data['render'],\n", + " transform=TOC_gt, color=[255,255,0])\n", + " \n", + " return\n", + "\n", + "plot_results(df)\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "63815a70", + "metadata": {}, + "outputs": [], + "source": [ + "# open meshcat viewer in notebook\n", + "vis.jupyter_cell()" + ] + }, + { + "cell_type": "markdown", + "id": "5d79c7aa", + "metadata": {}, + "source": [ + "## Print accuracy data" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "8eccf3e9", + "metadata": {}, + "outputs": [], + "source": [ + "df = all_infos\n", + "df_first_iter = df[df.iteration==1]\n", + "trans_err_init = float(df_first_iter.trans_err)\n", + "rot_err_init = float(df_first_iter.rot_err)\n", + "last_iter = df.iteration.max()\n", + "# print(\"last_iter\", last_iter)\n", + "df_final_iter = df[df.iteration==df.iteration.max()]\n", + "\n", + "trans_err = float(df_final_iter.trans_err)\n", + "rot_err = float(df_final_iter.rot_err)\n", + "# print(df_final_iter.tran)\n", + "# df_final_iter = df_final_iter.iloc[0]\n", + "\n", + "SO3_grid_size = pose_estimator._SO3_grid.shape[0]\n", + "# print(x_failure)\n", + "print(f\"result_name: {result_name}, SO3-grid-size={SO3_grid_size}\")\n", + "print(f\"iteration={last_iter}\")\n", + "print(f\"\\ninitial translation error (cm): {trans_err_init*100:.2f}\")\n", + "print(f\"initial rot_err (no sym) (deg): {rot_err_init:.1f}\")\n", + "print(f\"\\ntranslation error (cm): {trans_err*100:.2f}\\nrot_err (no sym) (deg): {rot_err:.1f}\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3 (ipykernel)", + "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.9.16" + } + }, + "nbformat": 4, + "nbformat_minor": 5 }