From 1bed642bd0d3e2ceab7d893e86ca3b0490151141 Mon Sep 17 00:00:00 2001
From: Rooholla-Khorrambakht <learn.kho@gmail.com>
Date: Mon, 27 May 2024 23:07:35 -0400
Subject: [PATCH] unitree lidar placement is fixed

---
 .../extrinsics/crrl-robot-may27.pkl           | Bin 1219 -> 1253 bytes
 examples/calibration_post_processing.ipynb    | 180 +++++++++++-------
 2 files changed, 114 insertions(+), 66 deletions(-)

diff --git a/Go2Py/assets/calibration/extrinsics/crrl-robot-may27.pkl b/Go2Py/assets/calibration/extrinsics/crrl-robot-may27.pkl
index 0907533f0fbb5c3d5c196629c827fad196debd59..03ed1b749dab754c8b995781543d060a10a726cd 100644
GIT binary patch
delta 134
zcmX@i`IM8Tfo1BgjVvlGVFEo&X%<sDJKCoNP0{dX&JgkQ^Yi)-1Yp9Op=3%@r*i`X
z7<{mY&<OJBTaM+d`m^>UL_n+$_LC>FNKRhB!o>^bz5V*G|NE+?5K#k0?#UZigaO&x
BDyRSe

delta 122
zcmV-=0EPeM3Bw5lfCQDeu>>RpWD|))fO7^gg{0~3zYlX88w5ADzXz-bFuy6!KXiGI
ziNbEkKiQ34p+Anczc6xM;soIDKeKXHZfiz%Kd??n&Q{A&zk`QD=nxvuzpSh6sSuWs
czm|a!<zWf$zbI$uL-s3xKa+t39+S)j8Oo72i2wiq

diff --git a/examples/calibration_post_processing.ipynb b/examples/calibration_post_processing.ipynb
index 230fd0c..1d0aa0b 100644
--- a/examples/calibration_post_processing.ipynb
+++ b/examples/calibration_post_processing.ipynb
@@ -537,7 +537,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 1,
+   "execution_count": 19,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -548,39 +548,47 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 4,
+   "execution_count": 20,
    "metadata": {},
    "outputs": [
     {
      "data": {
       "text/plain": [
-       "{'infra1_wrt_base': array([[-0.046, -0.071, 0.996, 0.322],\n",
-       "        [-0.999, -0.011, -0.047, 0.033],\n",
-       "        [0.014, -0.997, -0.071, 0.082],\n",
-       "        [0.000, 0.000, 0.000, 1.000]]),\n",
-       " 'imu_wrt_base': array([[1.000, -0.000, -0.021, -0.027],\n",
-       "        [-0.000, 1.000, -0.002, -0.005],\n",
-       "        [0.021, 0.002, 1.000, 0.040],\n",
-       "        [0.000, 0.000, 0.000, 1.000]]),\n",
-       " 'infra2_wrt_base': array([[-0.040, -0.070, 0.997, 0.318],\n",
-       "        [-0.999, -0.011, -0.041, -0.062],\n",
-       "        [0.014, -0.997, -0.070, 0.083],\n",
-       "        [0.000, 0.000, 0.000, 1.000]]),\n",
-       " 'color_wrt_base': array([[-0.040, -0.070, 0.997, 0.319],\n",
-       "        [-0.999, -0.008, -0.040, -0.026],\n",
-       "        [0.011, -0.998, -0.070, 0.082],\n",
-       "        [0.000, 0.000, 0.000, 1.000]]),\n",
-       " 'marker_wrt_base': array([[-0.888, 0.453, 0.076, 0.023],\n",
-       "        [-0.454, -0.891, -0.004, -0.000],\n",
-       "        [0.066, -0.038, 0.997, 0.058],\n",
-       "        [0.000, 0.000, 0.000, 1.000]]),\n",
-       " 'lidar_wrt_base': array([[-0.966, -0.087, 0.243, 0.191],\n",
-       "        [-0.088, 0.996, 0.005, -0.001],\n",
-       "        [-0.243, -0.016, -0.970, 0.008],\n",
-       "        [0.000, 0.000, 0.000, 1.000]])}"
+       "{'infra1_wrt_base': array([[-0.04633745, -0.07139989,  0.99637086,  0.32161791],\n",
+       "        [-0.99882303, -0.01099835, -0.04723963,  0.03330488],\n",
+       "        [ 0.01433134, -0.99738713, -0.07080622,  0.08162181],\n",
+       "        [ 0.        ,  0.        ,  0.        ,  1.        ]]),\n",
+       " 'imu_wrt_base': array([[ 9.99781597e-01, -2.84572838e-05, -2.08987443e-02,\n",
+       "         -2.73533177e-02],\n",
+       "        [-7.00270663e-06,  9.99998561e-01, -1.69667706e-03,\n",
+       "         -5.49944525e-03],\n",
+       "        [ 2.08987625e-02,  1.69645285e-03,  9.99780158e-01,\n",
+       "          3.99126757e-02],\n",
+       "        [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,\n",
+       "          1.00000000e+00]]),\n",
+       " 'infra2_wrt_base': array([[-0.04036617, -0.07029462,  0.9967092 ,  0.31796855],\n",
+       "        [-0.99908557, -0.01122962, -0.0412544 , -0.06191919],\n",
+       "        [ 0.01409262, -0.99746306, -0.06977704,  0.08277337],\n",
+       "        [ 0.        ,  0.        ,  0.        ,  1.        ]]),\n",
+       " 'color_wrt_base': array([[-0.03955514, -0.07002279,  0.99676085,  0.31941529],\n",
+       "        [-0.99915546, -0.00833421, -0.04023564, -0.02578968],\n",
+       "        [ 0.01112463, -0.99751058, -0.06963399,  0.08231088],\n",
+       "        [ 0.        ,  0.        ,  0.        ,  1.        ]]),\n",
+       " 'marker_wrt_base': array([[-8.88383872e-01,  4.52720962e-01,  7.62746821e-02,\n",
+       "          2.29437353e-02],\n",
+       "        [-4.54285606e-01, -8.90848853e-01, -3.59301716e-03,\n",
+       "         -6.99694933e-06],\n",
+       "        [ 6.63225789e-02, -3.78424687e-02,  9.97080369e-01,\n",
+       "          5.84578170e-02],\n",
+       "        [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,\n",
+       "          1.00000000e+00]]),\n",
+       " 'lidar_wrt_base': array([[-0.96602083, -0.08677698,  0.24346151,  0.19088012],\n",
+       "        [-0.08810804,  0.99609608,  0.00543825, -0.00108608],\n",
+       "        [-0.24298297, -0.01619745, -0.96989531,  0.00789635],\n",
+       "        [ 0.        ,  0.        ,  0.        ,  1.        ]])}"
       ]
      },
-     "execution_count": 4,
+     "execution_count": 20,
      "metadata": {},
      "output_type": "execute_result"
     }
@@ -591,39 +599,47 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 57,
+   "execution_count": 21,
    "metadata": {},
    "outputs": [
     {
      "data": {
       "text/plain": [
-       "{'infra1_wrt_base': array([[-0.046, -0.071, 0.996, 0.322],\n",
-       "        [-0.999, -0.011, -0.047, 0.033],\n",
-       "        [0.014, -0.997, -0.071, 0.082],\n",
-       "        [0.000, 0.000, 0.000, 1.000]]),\n",
-       " 'imu_wrt_base': array([[1.000, -0.000, -0.021, -0.027],\n",
-       "        [-0.000, 1.000, -0.002, -0.005],\n",
-       "        [0.021, 0.002, 1.000, 0.040],\n",
-       "        [0.000, 0.000, 0.000, 1.000]]),\n",
-       " 'infra2_wrt_base': array([[-0.040, -0.070, 0.997, 0.318],\n",
-       "        [-0.999, -0.011, -0.041, -0.062],\n",
-       "        [0.014, -0.997, -0.070, 0.083],\n",
-       "        [0.000, 0.000, 0.000, 1.000]]),\n",
-       " 'color_wrt_base': array([[-0.040, -0.070, 0.997, 0.319],\n",
-       "        [-0.999, -0.008, -0.040, -0.026],\n",
-       "        [0.011, -0.998, -0.070, 0.082],\n",
-       "        [0.000, 0.000, 0.000, 1.000]]),\n",
-       " 'marker_wrt_base': array([[-0.888, 0.453, 0.076, 0.023],\n",
-       "        [-0.454, -0.891, -0.004, -0.000],\n",
-       "        [0.066, -0.038, 0.997, 0.058],\n",
-       "        [0.000, 0.000, 0.000, 1.000]]),\n",
-       " 'lidar_wrt_base': array([[-0.966, -0.087, 0.243, 0.191],\n",
-       "        [-0.088, 0.996, 0.005, -0.001],\n",
-       "        [-0.243, -0.016, -0.970, 0.008],\n",
-       "        [0.000, 0.000, 0.000, 1.000]])}"
+       "{'infra1_wrt_base': array([[-0.04633745, -0.07139989,  0.99637086,  0.32161791],\n",
+       "        [-0.99882303, -0.01099835, -0.04723963,  0.03330488],\n",
+       "        [ 0.01433134, -0.99738713, -0.07080622,  0.08162181],\n",
+       "        [ 0.        ,  0.        ,  0.        ,  1.        ]]),\n",
+       " 'imu_wrt_base': array([[ 9.99781597e-01, -2.84572838e-05, -2.08987443e-02,\n",
+       "         -2.73533177e-02],\n",
+       "        [-7.00270663e-06,  9.99998561e-01, -1.69667706e-03,\n",
+       "         -5.49944525e-03],\n",
+       "        [ 2.08987625e-02,  1.69645285e-03,  9.99780158e-01,\n",
+       "          3.99126757e-02],\n",
+       "        [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,\n",
+       "          1.00000000e+00]]),\n",
+       " 'infra2_wrt_base': array([[-0.04036617, -0.07029462,  0.9967092 ,  0.31796855],\n",
+       "        [-0.99908557, -0.01122962, -0.0412544 , -0.06191919],\n",
+       "        [ 0.01409262, -0.99746306, -0.06977704,  0.08277337],\n",
+       "        [ 0.        ,  0.        ,  0.        ,  1.        ]]),\n",
+       " 'color_wrt_base': array([[-0.03955514, -0.07002279,  0.99676085,  0.31941529],\n",
+       "        [-0.99915546, -0.00833421, -0.04023564, -0.02578968],\n",
+       "        [ 0.01112463, -0.99751058, -0.06963399,  0.08231088],\n",
+       "        [ 0.        ,  0.        ,  0.        ,  1.        ]]),\n",
+       " 'marker_wrt_base': array([[-8.88383872e-01,  4.52720962e-01,  7.62746821e-02,\n",
+       "          2.29437353e-02],\n",
+       "        [-4.54285606e-01, -8.90848853e-01, -3.59301716e-03,\n",
+       "         -6.99694933e-06],\n",
+       "        [ 6.63225789e-02, -3.78424687e-02,  9.97080369e-01,\n",
+       "          5.84578170e-02],\n",
+       "        [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,\n",
+       "          1.00000000e+00]]),\n",
+       " 'lidar_wrt_base': array([[-0.96602083, -0.08677698,  0.24346151,  0.19088012],\n",
+       "        [-0.08810804,  0.99609608,  0.00543825, -0.00108608],\n",
+       "        [-0.24298297, -0.01619745, -0.96989531,  0.00789635],\n",
+       "        [ 0.        ,  0.        ,  0.        ,  1.        ]])}"
       ]
      },
-     "execution_count": 57,
+     "execution_count": 21,
      "metadata": {},
      "output_type": "execute_result"
     }
@@ -634,17 +650,17 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 41,
+   "execution_count": 22,
    "metadata": {},
    "outputs": [
     {
      "data": {
       "text/plain": [
-       "(array([0.319415, -0.025790, 0.082311]),\n",
-       " array([-1.640491, -0.011125, -1.610364]))"
+       "(array([ 0.31941529, -0.02578968,  0.08231088]),\n",
+       " array([-1.64049103, -0.01112486, -1.61036424]))"
       ]
      },
-     "execution_count": 41,
+     "execution_count": 22,
      "metadata": {},
      "output_type": "execute_result"
     }
@@ -665,27 +681,28 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 23,
    "metadata": {},
    "outputs": [
     {
      "name": "stderr",
      "output_type": "stream",
      "text": [
-      "[2024-05-27T22:53:16Z INFO  re_sdk_comms::server] Hosting a SDK server over TCP at 0.0.0.0:9876. Connect with the Rerun logging SDK.\n",
-      "[2024-05-27T22:53:17Z INFO  winit::platform_impl::platform::x11::window] Guessed window scale factor: 1\n"
+      "[2024-05-28T02:56:54Z WARN  re_sdk_comms::tcp_client] Tried to flush while TCP stream was still Pending. Data was possibly dropped.\n",
+      "[2024-05-28T02:56:54Z INFO  re_sdk_comms::server] Hosting a SDK server over TCP at 0.0.0.0:9876. Connect with the Rerun logging SDK.\n",
+      "[2024-05-28T02:56:54Z INFO  winit::platform_impl::platform::x11::window] Guessed window scale factor: 1\n",
+      "[2024-05-28T02:56:54Z INFO  re_sdk_comms::server] New SDK client connected: 127.0.0.1:37256\n",
+      "[2024-05-28T02:56:54Z INFO  re_sdk_comms::server] New SDK client connected: 127.0.0.1:37250\n"
      ]
     },
     {
      "name": "stderr",
      "output_type": "stream",
      "text": [
-      "[2024-05-27T22:53:17Z INFO  re_sdk_comms::server] New SDK client connected: 127.0.0.1:60992\n",
-      "[2024-05-27T22:53:17Z INFO  re_sdk_comms::server] New SDK client connected: 127.0.0.1:32770\n",
-      "[2024-05-27T22:53:17Z WARN  wgpu_hal::vulkan::instance] Unable to find extension: VK_EXT_swapchain_colorspace\n",
+      "[2024-05-28T02:56:54Z WARN  wgpu_hal::vulkan::instance] Unable to find extension: VK_EXT_swapchain_colorspace\n",
       "MESA-INTEL: warning: Performance support disabled, consider sysctl dev.i915.perf_stream_paranoid=0\n",
       "\n",
-      "[2024-05-27T22:53:17Z INFO  egui_wgpu] There were 3 available wgpu adapters: {backend: Vulkan, device_type: DiscreteGpu, name: \"NVIDIA GeForce GTX 1050\", driver: \"NVIDIA\", driver_info: \"535.171.04\", vendor: 0x10DE, device: 0x1C8D}, {backend: Vulkan, device_type: IntegratedGpu, name: \"Intel(R) HD Graphics 630 (KBL GT2)\", driver: \"Intel open-source Mesa driver\", driver_info: \"Mesa 21.2.6\", vendor: 0x8086, device: 0x591B}, {backend: Vulkan, device_type: Cpu, name: \"llvmpipe (LLVM 12.0.0, 256 bits)\", driver: \"llvmpipe\", driver_info: \"Mesa 21.2.6 (LLVM 12.0.0)\", vendor: 0x10005}\n"
+      "[2024-05-28T02:56:54Z INFO  egui_wgpu] There were 3 available wgpu adapters: {backend: Vulkan, device_type: DiscreteGpu, name: \"NVIDIA GeForce GTX 1050\", driver: \"NVIDIA\", driver_info: \"535.171.04\", vendor: 0x10DE, device: 0x1C8D}, {backend: Vulkan, device_type: IntegratedGpu, name: \"Intel(R) HD Graphics 630 (KBL GT2)\", driver: \"Intel open-source Mesa driver\", driver_info: \"Mesa 21.2.6\", vendor: 0x8086, device: 0x591B}, {backend: Vulkan, device_type: Cpu, name: \"llvmpipe (LLVM 12.0.0, 256 bits)\", driver: \"llvmpipe\", driver_info: \"Mesa 21.2.6 (LLVM 12.0.0)\", vendor: 0x10005}\n"
      ]
     }
    ],
@@ -697,7 +714,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 56,
+   "execution_count": 24,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -706,7 +723,31 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 52,
+   "execution_count": 25,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "t=[0.28945, 0, -0.046825]\n",
+    "R = Rotation.from_euler(seq='xyz', angles=[0, 2.8782, 0])\n",
+    "q = R.as_quat()\n",
+    "rr.log(\"world/base/lidar\", rr.Transform3D(translation=t, rotation=rr.Quaternion(xyzw=q)))"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 26,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "t=[-0.02557, 0, 0.04232]\n",
+    "R = Rotation.from_euler(seq='xyz', angles=[0, 0, 0])\n",
+    "q = R.as_quat()\n",
+    "rr.log(\"world/base/imu_bad\", rr.Transform3D(translation=t, rotation=rr.Quaternion(xyzw=q)))"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 27,
    "metadata": {},
    "outputs": [
     {
@@ -730,6 +771,13 @@
     "    qq = pin.Quaternion(T[0:3,0:3])\n",
     "    rr.log(f\"{parent}/{child}\", rr.Transform3D(translation=T[0:3,-1].tolist(), rotation=rr.Quaternion(xyzw=[qq.x,qq.y,qq.z, qq.w])))"
    ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": []
   }
  ],
  "metadata": {