diff --git a/Go2Py/assets/calibration/extrinsics/crrl-robot-may27.pkl b/Go2Py/assets/calibration/extrinsics/crrl-robot-may27.pkl index 0907533..03ed1b7 100644 Binary files a/Go2Py/assets/calibration/extrinsics/crrl-robot-may27.pkl and b/Go2Py/assets/calibration/extrinsics/crrl-robot-may27.pkl differ 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": {