2024-05-28 07:11:35 +08:00
|
|
|
{
|
|
|
|
|
"cells": [
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "markdown",
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"source": [
|
|
|
|
|
"# Calibration Postprocessing"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "markdown",
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"source": [
|
|
|
|
|
"## Kalibr (D455 to Body IMU)"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
2024-05-28 07:38:33 +08:00
|
|
|
"execution_count": 35,
|
2024-05-28 07:11:35 +08:00
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [],
|
|
|
|
|
"source": [
|
|
|
|
|
"from Go2Py.calibration import *\n",
|
|
|
|
|
"import numpy as np\n",
|
2024-05-28 07:38:33 +08:00
|
|
|
"np.set_printoptions(formatter={'float': lambda x: \"{0:0.6f}\".format(x)})"
|
2024-05-28 07:11:35 +08:00
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
|
|
|
|
"execution_count": 11,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [],
|
|
|
|
|
"source": [
|
|
|
|
|
"ext_params = [KalibrExtractExtrinsics(f'datasets/calibration/calibration/kalibr/dynamic/all/seq{i+1}.yaml') for i in range(5)]\n",
|
|
|
|
|
"int_params = KalibrExtractIntrinsics(f'datasets/calibration/calibration/kalibr/dynamic/all/seq{1}.yaml')"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
|
|
|
|
"execution_count": 12,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [],
|
|
|
|
|
"source": [
|
|
|
|
|
"def computeSE3Mean(poses):\n",
|
|
|
|
|
" T0 = poses[0]\n",
|
|
|
|
|
" log_delta_Ts = []\n",
|
|
|
|
|
" for T in poses:\n",
|
|
|
|
|
" log_delta_Ts.append(pin.log6(np.linalg.inv(T0)@T).vector)\n",
|
|
|
|
|
" log_delta_Ts = np.vstack(log_delta_Ts)\n",
|
|
|
|
|
" return T0@pin.exp6(np.mean(log_delta_Ts, axis=0)).homogeneous\n",
|
|
|
|
|
"\n",
|
|
|
|
|
"\n",
|
|
|
|
|
"ext_result = {}\n",
|
|
|
|
|
"int_result = {}\n",
|
|
|
|
|
"camera_name_map = {'cam0':'infra1',\n",
|
|
|
|
|
" 'cam1':'infra2',\n",
|
|
|
|
|
" 'cam2':'color'}\n",
|
|
|
|
|
"for cam in ['cam0', 'cam1', 'cam2']:\n",
|
|
|
|
|
" poses = [r[f'{cam}_T_imu'] for r in ext_params]\n",
|
|
|
|
|
" ext_result[f'{camera_name_map[cam]}_T_imu']=computeSE3Mean(poses)\n",
|
|
|
|
|
" int_result[camera_name_map[cam]]=int_params[cam]"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
|
|
|
|
"execution_count": 13,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [
|
|
|
|
|
{
|
|
|
|
|
"data": {
|
|
|
|
|
"text/plain": [
|
|
|
|
|
"{'infra1': {'camera_model': 'pinhole',\n",
|
|
|
|
|
" 'intrinsics': {'fu': 398.0379492735872,\n",
|
|
|
|
|
" 'fv': 401.10555839769535,\n",
|
|
|
|
|
" 'pu': 327.56389668833106,\n",
|
|
|
|
|
" 'pv': 244.66328819850395},\n",
|
|
|
|
|
" 'distortion_model': 'radtan',\n",
|
|
|
|
|
" 'distortion_coeffs': {'k1': 0.022437463738179255,\n",
|
|
|
|
|
" 'k2': -0.006800545399861931,\n",
|
|
|
|
|
" 'r1': 0.0007861230173005672,\n",
|
|
|
|
|
" 'r2': 0.0031405110415967147},\n",
|
|
|
|
|
" 'resolution': [640, 480]},\n",
|
|
|
|
|
" 'infra2': {'camera_model': 'pinhole',\n",
|
|
|
|
|
" 'intrinsics': {'fu': 398.170676888213,\n",
|
|
|
|
|
" 'fv': 401.14236478722404,\n",
|
|
|
|
|
" 'pu': 324.30936661646456,\n",
|
|
|
|
|
" 'pv': 244.30665138733275},\n",
|
|
|
|
|
" 'distortion_model': 'radtan',\n",
|
|
|
|
|
" 'distortion_coeffs': {'k1': 0.021509683195673927,\n",
|
|
|
|
|
" 'k2': -0.005685185848086602,\n",
|
|
|
|
|
" 'r1': 0.0005912786969070088,\n",
|
|
|
|
|
" 'r2': 0.0015930212849673176},\n",
|
|
|
|
|
" 'resolution': [640, 480]},\n",
|
|
|
|
|
" 'color': {'camera_model': 'pinhole',\n",
|
|
|
|
|
" 'intrinsics': {'fu': 389.87107372397037,\n",
|
|
|
|
|
" 'fv': 392.45741669539916,\n",
|
|
|
|
|
" 'pu': 323.75335233916996,\n",
|
|
|
|
|
" 'pv': 244.24311882848974},\n",
|
|
|
|
|
" 'distortion_model': 'radtan',\n",
|
|
|
|
|
" 'distortion_coeffs': {'k1': -0.03133853627816805,\n",
|
|
|
|
|
" 'k2': 0.04281860405665049,\n",
|
|
|
|
|
" 'r1': 0.00042180931009971407,\n",
|
|
|
|
|
" 'r2': 0.0024263659102242005},\n",
|
|
|
|
|
" 'resolution': [640, 480]}}"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
"execution_count": 13,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"output_type": "execute_result"
|
|
|
|
|
}
|
|
|
|
|
],
|
|
|
|
|
"source": [
|
|
|
|
|
"int_result"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
|
|
|
|
"execution_count": 14,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [
|
|
|
|
|
{
|
|
|
|
|
"data": {
|
|
|
|
|
"text/plain": [
|
|
|
|
|
"{'infra1_T_imu': array([[-0.046, -0.999, 0.017, 0.054],\n",
|
|
|
|
|
" [-0.092, -0.013, -0.996, 0.067],\n",
|
|
|
|
|
" [0.995, -0.047, -0.092, -0.343],\n",
|
|
|
|
|
" [0.000, 0.000, 0.000, 1.000]]),\n",
|
|
|
|
|
" 'infra2_T_imu': array([[-0.040, -0.999, 0.017, -0.043],\n",
|
|
|
|
|
" [-0.091, -0.013, -0.996, 0.066],\n",
|
|
|
|
|
" [0.995, -0.041, -0.091, -0.344],\n",
|
|
|
|
|
" [0.000, 0.000, 0.000, 1.000]]),\n",
|
|
|
|
|
" 'color_T_imu': array([[-0.039, -0.999, 0.014, -0.007],\n",
|
|
|
|
|
" [-0.091, -0.010, -0.996, 0.066],\n",
|
|
|
|
|
" [0.995, -0.040, -0.090, -0.344],\n",
|
|
|
|
|
" [0.000, 0.000, 0.000, 1.000]])}"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
"execution_count": 14,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"output_type": "execute_result"
|
|
|
|
|
}
|
|
|
|
|
],
|
|
|
|
|
"source": [
|
|
|
|
|
"ext_result"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "markdown",
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"source": [
|
|
|
|
|
"## Vicon2Gt"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
|
|
|
|
"execution_count": 15,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [
|
|
|
|
|
{
|
|
|
|
|
"data": {
|
|
|
|
|
"text/plain": [
|
|
|
|
|
"array([[-0.887, 0.452, 0.097, 0.051],\n",
|
|
|
|
|
" [-0.454, -0.891, -0.002, 0.006],\n",
|
|
|
|
|
" [0.086, -0.046, 0.995, 0.017],\n",
|
|
|
|
|
" [0.000, 0.000, 0.000, 1.000]])"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
"execution_count": 15,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"output_type": "execute_result"
|
|
|
|
|
}
|
|
|
|
|
],
|
|
|
|
|
"source": [
|
|
|
|
|
"imu_T_marker = Vicon2GtExtractParams('datasets/calibration/calibration/vicon2gt/seq1/seq1_vicon2gt_info.txt')\n",
|
|
|
|
|
"imu_T_marker"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "markdown",
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"source": [
|
|
|
|
|
"## Kinematics-Vicon"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
|
|
|
|
"execution_count": 16,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [],
|
|
|
|
|
"source": [
|
|
|
|
|
"import pickle\n",
|
|
|
|
|
"with open('datasets/calibration/calibration/kinematic-vicon-dataset/inplace_rotation.pkl', 'rb') as f:\n",
|
|
|
|
|
" data = pickle.load(f)"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
|
|
|
|
"execution_count": 17,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [],
|
|
|
|
|
"source": [
|
|
|
|
|
"from Go2Py.sim.mujoco import Go2Sim\n",
|
|
|
|
|
"robot = Go2Sim(dt=0.001)"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
|
|
|
|
"execution_count": 18,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [],
|
|
|
|
|
"source": [
|
|
|
|
|
"import time\n",
|
|
|
|
|
"import numpy as np\n",
|
|
|
|
|
"mjworld_Ts_base = []\n",
|
|
|
|
|
"vworld_Ts_marker = []\n",
|
|
|
|
|
"Kp = 200\n",
|
|
|
|
|
"Kv = 5.0\n",
|
|
|
|
|
"robot.standUpReset()\n",
|
|
|
|
|
"for i in range(len(data['q'])):\n",
|
|
|
|
|
" state = robot.getJointStates()\n",
|
|
|
|
|
" tau = Kp*np.eye(12)@(data['q'][i] - state['q']).reshape(12,1)+data['tau'][i].reshape(12,1) + \\\n",
|
|
|
|
|
" Kv*np.eye(12)@(data['dq'][i] - state['dq']).reshape(12,1)\n",
|
|
|
|
|
" robot.setCommands(np.zeros(12), np.zeros(12), np.zeros(12), np.zeros(12), tau)\n",
|
|
|
|
|
"\n",
|
|
|
|
|
" t, q = robot.getPose()\n",
|
|
|
|
|
" R = pin.Quaternion(np.hstack([q[1:], q[0]])).matrix()\n",
|
|
|
|
|
" mjworld_T_base=np.vstack([np.hstack([R, t.reshape(3,1)]), np.array([0,0,0,1])])\n",
|
|
|
|
|
" vworld_T_marker = data['world_T_marker'][i]\n",
|
|
|
|
|
" mjworld_Ts_base.append(mjworld_T_base.reshape(1,4,4))\n",
|
|
|
|
|
" vworld_Ts_marker.append(vworld_T_marker.reshape(1,4,4))\n",
|
|
|
|
|
" robot.step()\n"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
|
|
|
|
"execution_count": 20,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [],
|
|
|
|
|
"source": [
|
|
|
|
|
"mjworld_Ts_base = np.vstack(mjworld_Ts_base[:-3000])\n",
|
|
|
|
|
"vworld_Ts_marker = np.vstack(vworld_Ts_marker[:-3000])"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
|
|
|
|
"execution_count": 21,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [
|
|
|
|
|
{
|
|
|
|
|
"data": {
|
|
|
|
|
"text/plain": [
|
|
|
|
|
"[<matplotlib.lines.Line2D at 0x7f816c43a4f0>,\n",
|
|
|
|
|
" <matplotlib.lines.Line2D at 0x7f816c43a550>,\n",
|
|
|
|
|
" <matplotlib.lines.Line2D at 0x7f816c43a670>]"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
"execution_count": 21,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"output_type": "execute_result"
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"data": {
|
|
|
|
|
"image/png": "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
|
|
|
|
|
"text/plain": [
|
|
|
|
|
"<Figure size 432x288 with 1 Axes>"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
"metadata": {
|
|
|
|
|
"needs_background": "light"
|
|
|
|
|
},
|
|
|
|
|
"output_type": "display_data"
|
|
|
|
|
}
|
|
|
|
|
],
|
|
|
|
|
"source": [
|
|
|
|
|
"import matplotlib.pyplot as plt\n",
|
|
|
|
|
"plt.plot(vworld_Ts_marker[:,0:3,-1])"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
|
|
|
|
"execution_count": 22,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [
|
|
|
|
|
{
|
|
|
|
|
"data": {
|
|
|
|
|
"text/plain": [
|
|
|
|
|
"[<matplotlib.lines.Line2D at 0x7f8169306220>,\n",
|
|
|
|
|
" <matplotlib.lines.Line2D at 0x7f8169306280>,\n",
|
|
|
|
|
" <matplotlib.lines.Line2D at 0x7f81693063a0>]"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
"execution_count": 22,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"output_type": "execute_result"
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"data": {
|
|
|
|
|
"image/png": "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
|
|
|
|
|
"text/plain": [
|
|
|
|
|
"<Figure size 432x288 with 1 Axes>"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
"metadata": {
|
|
|
|
|
"needs_background": "light"
|
|
|
|
|
},
|
|
|
|
|
"output_type": "display_data"
|
|
|
|
|
}
|
|
|
|
|
],
|
|
|
|
|
"source": [
|
|
|
|
|
"plt.plot(mjworld_Ts_base[:,0:3,-1])"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
|
|
|
|
"execution_count": 23,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [],
|
|
|
|
|
"source": [
|
|
|
|
|
"from SimpleHandEye.solvers import OpenCVSolver"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
|
|
|
|
"execution_count": 24,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [],
|
|
|
|
|
"source": [
|
|
|
|
|
"solver = OpenCVSolver()"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
|
|
|
|
"execution_count": 25,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [],
|
|
|
|
|
"source": [
|
|
|
|
|
"A = mjworld_Ts_base\n",
|
|
|
|
|
"B = vworld_Ts_marker\n",
|
|
|
|
|
"X , Y = solver.solve(A = A, B = B)"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
|
|
|
|
"execution_count": 26,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [
|
|
|
|
|
{
|
|
|
|
|
"data": {
|
|
|
|
|
"text/plain": [
|
|
|
|
|
"[<matplotlib.lines.Line2D at 0x7f815c364850>,\n",
|
|
|
|
|
" <matplotlib.lines.Line2D at 0x7f815c364880>,\n",
|
|
|
|
|
" <matplotlib.lines.Line2D at 0x7f815c3649a0>]"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
"execution_count": 26,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"output_type": "execute_result"
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"data": {
|
|
|
|
|
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAXQAAAD4CAYAAAD8Zh1EAAAAOXRFWHRTb2Z0d2FyZQBNYXRwbG90bGliIHZlcnNpb24zLjUuMiwgaHR0cHM6Ly9tYXRwbG90bGliLm9yZy8qNh9FAAAACXBIWXMAAAsTAAALEwEAmpwYAABmj0lEQVR4nO2dd3hcxdWH37nbm3rvLnLvDdMx1ZgaekLoCSWUQBIIhCSk9wJ8kNBD79X0YgwYG4x7L3KRrd6llbbv3vv9cde2ZK2qZQut530ePdq9d+7szO7d386cOeeM0DQNiUQikQx9lMFugEQikUgGBinoEolEEidIQZdIJJI4QQq6RCKRxAlS0CUSiSROMA7WC6elpWlFRUWD9fISiUQyJFmxYkW9pmnpsc4NmqAXFRWxfPnywXp5iUQiGZIIIXZ1dU6aXCQSiSROkIIukUgkcYIUdIlEIokTeiXoQoi5QogtQohtQog7uyl3vhBCE0LMGLgmSiQSiaQ39CjoQggD8CBwOjAO+K4QYlyMci7gx8DSgW6kRCKRSHqmNyP0WcA2TdN2aJoWBF4EzolR7vfAXwH/ALZPIpFIJL2kN4KeC5S1e14ePbYXIcQ0IF/TtHe7q0gIca0QYrkQYnldXV2fGyuRSCSSrjlgP3QhhAL8C7iyp7Kapj0CPAIwY8aMuMnbu7p2NStqVpBuT6cooYhJ6ZMGu0kSieQgEFEjfLzrY0JqCH9EN0YUJxUzJWPK4DYsSm8EvQLIb/c8L3psDy5gAvCZEAIgC5gvhDhb07QhFzl09/zb2LV5LZpBQTEZOXb0SVxx7A1Y7I6Y5VVN5fG/346rWVAiNL5UNDKdWWS5ckhJy+Kka27AbLUd4l5IJJKDwRNv/o2aN75AExqqAFXRWKvA564M8vOKGXvk8Yw9+vhBa19vBH0ZUCyEGIYu5JcA39tzUtO0FiBtz3MhxGfAz4aimO9u2kXKcyWksE+AWxd/xgNPfs4t/3sFk9Xa+ZqGUop2WwDInDCO8ubdVHuqcTfUkbBxI0nDCjly3vmHrA8SieTgUbN0NbaggaJjjsKAIBQMsq52LaWeSsLLmti57BtGzz4GxWAYlPb1KOiapoWFEDcBHwIG4AlN0zYIIX4HLNc0bf7BbmR7Fu5eyBvb3uDeOfeiiIF1o9+wWXfQKZhzNCeeeSl+n5fHXv4jzrVNbF2/jPEzju10TcmONQCMOG8u5158EwA7Wnbw8oYX4b4lbNi8VAq6RBIniAYfIbvC+Tf/Yu+xCzSNdfXreOKxe8hfHaK5toqU7LxBaV+vFFHTtPc0TRuladoITdP+GD3261hirmnaCQdzdF7nq2Nh2UIq2yoHvO6GGt2SNGLmEaTmFZBbPIYj554HwI6yjTGvaawqByC7qHjvseGJw7l11k8A8Aa9A95OieTbwIL3nmPjisWD3YxDisEfQSR2NKEKIZiUPonJI48AoKquLNalh4QhFymaYc8AoCXQMuB1t1Togp6fP3rvsYKcUQDU746dD8fjbwUgJTWrw3Gr0UqrM0Kgqn7A23k48fa7j/OXH53PjpK1vSrvD/v55Zu38crHj9DW2HCQW3f48s2KT1j91Au8/7c/42tr7dO1mqbx2pKnWLH2M8q2bCAU6J2nsxqJEA6FiIRD1JbuYNfa1fg9bf1pfr8Ih0OYQgJTamLM86mZuvNfdVXpIWvT/gxatsX+YjfaAfCGB37k621sQgHS0rL3HstK1j8kvxqIeU1bte5+mZGS0+mcUVUIGw+tM0/pjg18vXA+FouNvJRCvP42SnaspWnlJk69524mjz6y33V73M24G+vIyCvCYDQB4Pe0sX7pZ0yfcwbRRfEBQ9M0Vr31Bo4WeOvRf3LrX/6HULofg3y07m2SXyhhNyU8/Nh8jr7uWmafePaAtksCS957Ze/jp351K0eeeSGjjjgam9PV47Xvr3mD0vteoTT6XFMEc265mamzTiISDmEwmVCUjjZoTdO478ffR61rRZiMaKEwANbsNG6898kB6lX3VNbrg7qE7KyY5xOcKQD4BnFWPvQE3RQV9NDAv2mBSAAbdBCNREsiPksEKmtjX6MFUABXUkrnc4kGbMHwgLezK75e9A6LH3ho7/Md7c4pwMf//Adrx4yh1l+Hze7kmuv+gMVmj1nX//3tZgJrd4HJQOqUsUydfTIfP/x/KJ4QAJrFgDCboFUfXX392duYFRMefxshNcT42Sdwxnk/7LKtG3etoq2unrGjphNs9fDOi/8haIgwfOJ0Jk8+lrS0HFas/hRHdCKm7mrgH1d9BwSEjTDjuitJys8jy5FFrjN373rK2gUfYAM0uxHhDbP4teekoPeBcChIS6CFSEMrX773Co68TIqGj2dE8WSMZvPect7SKowG2FrsY9TWaj555AE+eeNJLv3LvbQEWyhKLMJisMR8jRUL3sUOhFOt+Kak4VpQzqf/fYCF/30QEYgAoKQ40dDQVBVNVRHufQMqLRRm5YRWJmx2QFU94VAIo8l0UN8XgIraUgASHEkxzycl6ynK3Q2xteJQMOQE3WbU7Ve+sG/A69YaPfgTO44AhRAoCMIGNeY14boWFIPWaUQBIIwGTHWxR/YDhaqpvLDwEXZuXI1tkW7Pz7v+HFxp6ZS4t5HiSCPHlcOX9/8XZ3WAymWrAPCpCvNT/8uFl/20U52N7jqCK3YiALc5iFiylgVL1qIAbnuIwJhk2uobiERCmNIs5O0yEthSQa09RNiokeAxsvmltzjymDNIyeg8c/lm5Scs+uu9ACxq3xehsfqrjazmGSIGDUNEH/FPu/tHvLPoOZRdzQS1EJlVRtb+43HqEwOU5LURchgwFqWhmIwM29aMDSs/ffx1/nDDOVhaB/4+iVceevQXeD7pbNpaz6sARMwCgUCEVSwqGI8ayR9+cCcLdy1k3cKPSP2ijmevuQaBoDrFT9NRaeQNG43LmoBJMaGhEdEi+DeVYTVZ+fl/9Hr/uuZCjPX65+RLEPhEEK+hjpBRBSFAEaRHzDg9BlpOzsWZncmT837FI0/8Cv/H6ynbuYlho3oX+/HOJ0+z4vVXSUjP4IY77sXqcPb6/amu1kfombnDYp5PTtAF3R8YvHtuyAn6HpNLvW/gbdMhNYwj0lmYvYkKCbWemNcECeOMxDY1mDQDYdOBj9B9Yd/eH7L9eerFv9D45pK9jpYiO4GL53QeGRffOZy333mUscedxIS8KTxz9dVUVuzoVA5g5QZdZgvPPZlzLrqR1+b/h4oXPwbgop//nvFjZqFqKqqmYlSMeIMelmxYQE7+SIYlDeODT59nx+NvsOTLtznzvOs61b9i0QcA2I4ZQ8Co4sFHZvEojp9wGps2L2P3lvU01lXhF0FGzzyaOZPmMWfSvL3X79q5iVfvvJ20FgtpLdFR4DdhIAxYSR8zSp9l5SZh2NCkj/B6MNUc7tR5amn8fDUWFJQMF63+VrSxGUw/6jQqdm+lclcJfncrYSKEhIY5wclPr7ydLEcW3x33XS4acxH3em8i0uLB4Yasmkay3mkDVgDQao6gqAKDKkhRzaRPGbP3tbMnjqduoe5HcdfDb6Ki4g/7sRltGJWuJSp32Gi2s56SkjW9FvQNr7+Fs0FFbajmg9cf49zLbu31e9TYVA1AenJ2zPOJjmQ0NIKhwct+MuQEPdLiIa/GxsKnHqFy6jrSXBlowTChsnocI/OYXnw044r7l+zR2hJB5HZe8DBpBiJdvFORYAh/cuyTWrINUdv/D/eh//2S6qWriYRDXPTrPzOxYFqnMhWLvsEGnPvzeyhZs5RjvvO9zhUBBdkjufGHf9373G9VMdU2xiy7c9s6ACZMPhqTwcRF59zEvz9cjCnJyfgxswBQhLLXzGE3Ozh56j6zxvTpJ7Lj8TeoLNses/7G3buxAj+6+R+dzo3KGgcnxLxsL4XDxvLTl94hEg5Rva2Eii0b0TSNcDBI0Oth1rkX6u1KTQaa8LQ040zubBKT7GPh6nexhBQmXXYRp5x5eceTs3u+3qAY+Ont/937vKF8NxWbN9JcU4WnuQmT1YbBaMRgNGI0m5l8yr4f6NPP+yE
|
|
|
|
|
"text/plain": [
|
|
|
|
|
"<Figure size 432x288 with 1 Axes>"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
"metadata": {
|
|
|
|
|
"needs_background": "light"
|
|
|
|
|
},
|
|
|
|
|
"output_type": "display_data"
|
|
|
|
|
}
|
|
|
|
|
],
|
|
|
|
|
"source": [
|
|
|
|
|
"t1 = np.array([A[i]@X for i in range(A.shape[0])])[:,0:3,-1]\n",
|
|
|
|
|
"t2 = np.array([Y@B[i] for i in range(B.shape[0])])[:,0:3,-1]\n",
|
|
|
|
|
"plt.plot(t1)\n",
|
|
|
|
|
"plt.plot(t2)"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
|
|
|
|
"execution_count": 27,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [
|
|
|
|
|
{
|
|
|
|
|
"data": {
|
|
|
|
|
"text/plain": [
|
|
|
|
|
"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]])"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
"execution_count": 27,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"output_type": "execute_result"
|
|
|
|
|
}
|
|
|
|
|
],
|
|
|
|
|
"source": [
|
|
|
|
|
"base_T_marker = X\n",
|
|
|
|
|
"base_T_marker\n"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "markdown",
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"source": [
|
|
|
|
|
"# Load Lidar to Camera extrinsics"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
|
|
|
|
"execution_count": 40,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [
|
|
|
|
|
{
|
|
|
|
|
"data": {
|
|
|
|
|
"text/plain": [
|
|
|
|
|
"array([[0.124, 0.311, -0.942, -0.140],\n",
|
|
|
|
|
" [-0.992, 0.014, -0.125, -0.037],\n",
|
|
|
|
|
" [-0.026, 0.950, 0.310, -0.041],\n",
|
|
|
|
|
" [0.000, 0.000, 0.000, 1.000]])"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
"execution_count": 40,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"output_type": "execute_result"
|
|
|
|
|
}
|
|
|
|
|
],
|
|
|
|
|
"source": [
|
|
|
|
|
"import json\n",
|
|
|
|
|
"with open('datasets/calibration/calibration/utlidar-camera/calib.json', 'r') as f: \n",
|
|
|
|
|
" ext = json.load(f)\n",
|
|
|
|
|
"lidar_T_camera = ext['results']['init_T_lidar_camera']\n",
|
|
|
|
|
"t = np.array(lidar_T_camera[0:3])\n",
|
|
|
|
|
"q = np.array(lidar_T_camera[3:])\n",
|
|
|
|
|
"R = pin.Quaternion(q).matrix()\n",
|
|
|
|
|
"lidar_T_camera=np.vstack([np.hstack([R, t.reshape(3,1)]), np.array([0,0,0,1])])\n",
|
|
|
|
|
"lidar_T_camera"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "markdown",
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"source": [
|
|
|
|
|
"# Combine it All"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
|
|
|
|
"execution_count": 41,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [
|
|
|
|
|
{
|
|
|
|
|
"data": {
|
|
|
|
|
"text/plain": [
|
|
|
|
|
"{'infra1_T_imu': array([[-0.046, -0.999, 0.017, 0.054],\n",
|
|
|
|
|
" [-0.092, -0.013, -0.996, 0.067],\n",
|
|
|
|
|
" [0.995, -0.047, -0.092, -0.343],\n",
|
|
|
|
|
" [0.000, 0.000, 0.000, 1.000]]),\n",
|
|
|
|
|
" 'infra2_T_imu': array([[-0.040, -0.999, 0.017, -0.043],\n",
|
|
|
|
|
" [-0.091, -0.013, -0.996, 0.066],\n",
|
|
|
|
|
" [0.995, -0.041, -0.091, -0.344],\n",
|
|
|
|
|
" [0.000, 0.000, 0.000, 1.000]]),\n",
|
|
|
|
|
" 'color_T_imu': array([[-0.039, -0.999, 0.014, -0.007],\n",
|
|
|
|
|
" [-0.091, -0.010, -0.996, 0.066],\n",
|
|
|
|
|
" [0.995, -0.040, -0.090, -0.344],\n",
|
|
|
|
|
" [0.000, 0.000, 0.000, 1.000]]),\n",
|
|
|
|
|
" 'base_T_marker': 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",
|
|
|
|
|
" 'imu_T_marker': array([[-0.887, 0.452, 0.097, 0.051],\n",
|
|
|
|
|
" [-0.454, -0.891, -0.002, 0.006],\n",
|
|
|
|
|
" [0.086, -0.046, 0.995, 0.017],\n",
|
|
|
|
|
" [0.000, 0.000, 0.000, 1.000]]),\n",
|
|
|
|
|
" 'lidar_T_color': array([[0.124, 0.311, -0.942, -0.140],\n",
|
|
|
|
|
" [-0.992, 0.014, -0.125, -0.037],\n",
|
|
|
|
|
" [-0.026, 0.950, 0.310, -0.041],\n",
|
|
|
|
|
" [0.000, 0.000, 0.000, 1.000]])}"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
"execution_count": 41,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"output_type": "execute_result"
|
|
|
|
|
}
|
|
|
|
|
],
|
|
|
|
|
"source": [
|
|
|
|
|
"ext_result['base_T_marker'] = base_T_marker\n",
|
|
|
|
|
"ext_result['imu_T_marker'] = imu_T_marker\n",
|
|
|
|
|
"ext_result['lidar_T_color'] = lidar_T_camera\n",
|
|
|
|
|
"ext_result"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
|
|
|
|
"execution_count": 42,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [],
|
|
|
|
|
"source": [
|
|
|
|
|
"from Go2Py.calibration import ExtrinsicCalibrationManager\n",
|
|
|
|
|
"manager = ExtrinsicCalibrationManager()"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
|
|
|
|
"execution_count": 43,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [],
|
|
|
|
|
"source": [
|
|
|
|
|
"for key, T in zip(ext_result.keys(), ext_result.values()):\n",
|
|
|
|
|
" parent = key.split('_T_')[0]\n",
|
|
|
|
|
" child = key.split('_T_')[1]\n",
|
|
|
|
|
" manager.add(parent=parent, child=child, R=T[0:3,0:3], t=T[0:3,-1])"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
|
|
|
|
"execution_count": 44,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [],
|
|
|
|
|
"source": [
|
|
|
|
|
"ext_wrt_base = manager.get_all('base')\n",
|
|
|
|
|
"ext_wrt_imu = manager.get_all('imu')\n",
|
|
|
|
|
"result = {'ext_wrt_base':ext_wrt_base,\n",
|
|
|
|
|
" 'ext_wrt_imu':ext_wrt_base}"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
2024-05-28 11:08:56 +08:00
|
|
|
"execution_count": 37,
|
2024-05-28 07:11:35 +08:00
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [],
|
|
|
|
|
"source": [
|
|
|
|
|
"with open('datasets/calibration/final_results.pkl', 'wb') as f:\n",
|
|
|
|
|
" pickle.dump(result, f)"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
2024-05-28 11:07:35 +08:00
|
|
|
"execution_count": 19,
|
2024-05-28 07:11:35 +08:00
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [],
|
|
|
|
|
"source": [
|
|
|
|
|
"import pickle\n",
|
|
|
|
|
"with open('datasets/calibration/final_results.pkl', 'rb') as f:\n",
|
|
|
|
|
" result = pickle.load(f)"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
2024-05-28 11:07:35 +08:00
|
|
|
"execution_count": 20,
|
2024-05-28 07:11:35 +08:00
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [
|
|
|
|
|
{
|
|
|
|
|
"data": {
|
|
|
|
|
"text/plain": [
|
2024-05-28 11:07:35 +08:00
|
|
|
"{'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. ]])}"
|
2024-05-28 07:11:35 +08:00
|
|
|
]
|
|
|
|
|
},
|
2024-05-28 11:07:35 +08:00
|
|
|
"execution_count": 20,
|
2024-05-28 07:11:35 +08:00
|
|
|
"metadata": {},
|
|
|
|
|
"output_type": "execute_result"
|
|
|
|
|
}
|
|
|
|
|
],
|
|
|
|
|
"source": [
|
|
|
|
|
"result['ext_wrt_base']"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
2024-05-28 11:07:35 +08:00
|
|
|
"execution_count": 21,
|
2024-05-28 07:11:35 +08:00
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [
|
|
|
|
|
{
|
|
|
|
|
"data": {
|
|
|
|
|
"text/plain": [
|
2024-05-28 11:07:35 +08:00
|
|
|
"{'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. ]])}"
|
2024-05-28 07:11:35 +08:00
|
|
|
]
|
|
|
|
|
},
|
2024-05-28 11:07:35 +08:00
|
|
|
"execution_count": 21,
|
2024-05-28 07:11:35 +08:00
|
|
|
"metadata": {},
|
|
|
|
|
"output_type": "execute_result"
|
|
|
|
|
}
|
|
|
|
|
],
|
|
|
|
|
"source": [
|
|
|
|
|
"result['ext_wrt_imu']"
|
|
|
|
|
]
|
|
|
|
|
},
|
2024-05-28 07:38:33 +08:00
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
2024-05-28 11:07:35 +08:00
|
|
|
"execution_count": 22,
|
2024-05-28 07:38:33 +08:00
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [
|
|
|
|
|
{
|
|
|
|
|
"data": {
|
|
|
|
|
"text/plain": [
|
2024-05-28 11:07:35 +08:00
|
|
|
"(array([ 0.31941529, -0.02578968, 0.08231088]),\n",
|
|
|
|
|
" array([-1.64049103, -0.01112486, -1.61036424]))"
|
2024-05-28 07:38:33 +08:00
|
|
|
]
|
|
|
|
|
},
|
2024-05-28 11:07:35 +08:00
|
|
|
"execution_count": 22,
|
2024-05-28 07:38:33 +08:00
|
|
|
"metadata": {},
|
|
|
|
|
"output_type": "execute_result"
|
|
|
|
|
}
|
|
|
|
|
],
|
|
|
|
|
"source": [
|
|
|
|
|
"import pinocchio as pin\n",
|
|
|
|
|
"T = result['ext_wrt_imu']['color_wrt_base']\n",
|
|
|
|
|
"from scipy.spatial.transform import Rotation \n",
|
|
|
|
|
"T[0:3,-1], Rotation.from_matrix(T[0:3,0:3]).as_euler(seq='xyz')"
|
|
|
|
|
]
|
|
|
|
|
},
|
2024-05-28 07:11:35 +08:00
|
|
|
{
|
|
|
|
|
"cell_type": "markdown",
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"source": [
|
|
|
|
|
"## 3D Visualizations"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
2024-05-28 11:07:35 +08:00
|
|
|
"execution_count": 23,
|
2024-05-28 07:11:35 +08:00
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [
|
|
|
|
|
{
|
|
|
|
|
"name": "stderr",
|
|
|
|
|
"output_type": "stream",
|
|
|
|
|
"text": [
|
2024-05-28 11:07:35 +08:00
|
|
|
"[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"
|
2024-05-28 07:11:35 +08:00
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"name": "stderr",
|
|
|
|
|
"output_type": "stream",
|
|
|
|
|
"text": [
|
2024-05-28 11:07:35 +08:00
|
|
|
"[2024-05-28T02:56:54Z WARN wgpu_hal::vulkan::instance] Unable to find extension: VK_EXT_swapchain_colorspace\n",
|
2024-05-28 07:11:35 +08:00
|
|
|
"MESA-INTEL: warning: Performance support disabled, consider sysctl dev.i915.perf_stream_paranoid=0\n",
|
|
|
|
|
"\n",
|
2024-05-28 11:07:35 +08:00
|
|
|
"[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"
|
2024-05-28 07:11:35 +08:00
|
|
|
]
|
|
|
|
|
}
|
|
|
|
|
],
|
|
|
|
|
"source": [
|
|
|
|
|
"import rerun as rr\n",
|
|
|
|
|
"rr.init('ext_visualization')\n",
|
|
|
|
|
"rr.spawn()"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
2024-05-28 11:07:35 +08:00
|
|
|
"execution_count": 24,
|
2024-05-28 07:11:35 +08:00
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [],
|
|
|
|
|
"source": [
|
|
|
|
|
"rr.log(\"world/base\", rr.Transform3D(translation=[0,0,0], rotation=rr.Quaternion(xyzw=[0,0,0,1])))"
|
|
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
2024-05-28 11:07:35 +08:00
|
|
|
"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",
|
2024-05-28 11:08:56 +08:00
|
|
|
"execution_count": 35,
|
2024-05-28 11:07:35 +08:00
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [],
|
|
|
|
|
"source": [
|
2024-05-28 11:08:56 +08:00
|
|
|
"import numpy as np\n",
|
|
|
|
|
"result['ext_wrt_base']['lidar_wrt_base']=np.vstack([np.hstack([R.as_matrix(), np.array(t).reshape(3,1)]), np.array([0,0,0,1])])"
|
2024-05-28 11:07:35 +08:00
|
|
|
]
|
|
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
|
|
|
|
"execution_count": 27,
|
2024-05-28 07:11:35 +08:00
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [
|
|
|
|
|
{
|
|
|
|
|
"name": "stdout",
|
|
|
|
|
"output_type": "stream",
|
|
|
|
|
"text": [
|
|
|
|
|
"base infra1\n",
|
|
|
|
|
"base imu\n",
|
|
|
|
|
"base infra2\n",
|
|
|
|
|
"base color\n",
|
|
|
|
|
"base marker\n",
|
|
|
|
|
"base lidar\n"
|
|
|
|
|
]
|
|
|
|
|
}
|
|
|
|
|
],
|
|
|
|
|
"source": [
|
|
|
|
|
"import pinocchio as pin\n",
|
|
|
|
|
"for key, T in result['ext_wrt_base'].items():\n",
|
|
|
|
|
" child, parent = key.split('_wrt_')\n",
|
|
|
|
|
" print(parent, child)\n",
|
|
|
|
|
" 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])))"
|
|
|
|
|
]
|
2024-05-28 11:07:35 +08:00
|
|
|
},
|
|
|
|
|
{
|
|
|
|
|
"cell_type": "code",
|
|
|
|
|
"execution_count": null,
|
|
|
|
|
"metadata": {},
|
|
|
|
|
"outputs": [],
|
|
|
|
|
"source": []
|
2024-05-28 07:11:35 +08:00
|
|
|
}
|
|
|
|
|
],
|
|
|
|
|
"metadata": {
|
|
|
|
|
"kernelspec": {
|
|
|
|
|
"display_name": "Python 3",
|
|
|
|
|
"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.8.10"
|
|
|
|
|
}
|
|
|
|
|
},
|
|
|
|
|
"nbformat": 4,
|
|
|
|
|
"nbformat_minor": 4
|
|
|
|
|
}
|
