Corrected Euclidean Error Function, Dahsboard Changes

stretch_camera_testrig/config/test1.yaml

@@ -1,196 +0,0 @@
-markers:
-  -
-    header:
-      seq: 0
-      stamp:
-        secs: 1651003161
-        nsecs: 766932964
-      frame_id: "/camera_color_optical_frame"
-    ns: ''
-    id: 130
-    type: 1
-    action: 0
-    pose:
-      position:
-        x: 0.0241800574487
-        y: -0.0342552404967
-        z: 0.969235731015
-      orientation:
-        x: 0.999344598409
-        y: -0.00832851725226
-        z: -0.0112885233133
-        w: 0.0333703262417
-    scale:
-      x: 0.047
-      y: 0.047
-      z: 0.005
-    color:
-      r: 1.0
-      g: 0.0
-      b: 0.705882370472
-      a: 0.330000013113
-    lifetime:
-      secs: 0
-      nsecs: 200000000
-    frame_locked: False
-    points: []
-    colors: []
-    text: "base_left"
-    mesh_resource: ''
-    mesh_use_embedded_materials: False
-  -
-    header:
-      seq: 0
-      stamp:
-        secs: 1651003161
-        nsecs: 766932964
-      frame_id: "/camera_color_optical_frame"
-    ns: ''
-    id: 131
-    type: 1
-    action: 0
-    pose:
-      position:
-        x: 0.0920780699485
-        y: -0.0335624826098
-        z: 0.968065369117
-      orientation:
-        x: 0.999093130014
-        y: -0.00356607996036
-        z: -0.0204699706518
-        w: 0.0371642426745
-    scale:
-      x: 0.047
-      y: 0.047
-      z: 0.005
-    color:
-      r: 0.611764729023
-      g: 0.0
-      b: 1.0
-      a: 0.330000013113
-    lifetime:
-      secs: 0
-      nsecs: 200000000
-    frame_locked: False
-    points: []
-    colors: []
-    text: "base_right"
-    mesh_resource: ''
-    mesh_use_embedded_materials: False
-  -
-    header:
-      seq: 0
-      stamp:
-        secs: 1651003161
-        nsecs: 766932964
-      frame_id: "/camera_color_optical_frame"
-    ns: ''
-    id: 132
-    type: 1
-    action: 0
-    pose:
-      position:
-        x: 0.0129902026683
-        y: -0.0928715676789
-        z: 0.972609772983
-      orientation:
-        x: 0.992153145926
-        y: 0.0146480995898
-        z: -0.0982938952781
-        w: 0.0758675052842
-    scale:
-      x: 0.0235
-      y: 0.0235
-      z: 0.005
-    color:
-      r: 0.0
-      g: 0.0705882385373
-      b: 1.0
-      a: 0.330000013113
-    lifetime:
-      secs: 0
-      nsecs: 200000000
-    frame_locked: False
-    points: []
-    colors: []
-    text: "wrist_inside"
-    mesh_resource: ''
-    mesh_use_embedded_materials: False
-  -
-    header:
-      seq: 0
-      stamp:
-        secs: 1651003161
-        nsecs: 766932964
-      frame_id: "/camera_color_optical_frame"
-    ns: ''
-    id: 133
-    type: 1
-    action: 0
-    pose:
-      position:
-        x: 0.0544769741546
-        y: -0.092754424718
-        z: 0.970924466667
-      orientation:
-        x: 0.999674792188
-        y: -0.00628684941704
-        z: 0.0109935765558
-        w: -0.0221342870211
-    scale:
-      x: 0.0235
-      y: 0.0235
-      z: 0.005
-    color:
-      r: 0.0
-      g: 0.752941191196
-      b: 1.0
-      a: 0.330000013113
-    lifetime:
-      secs: 0
-      nsecs: 200000000
-    frame_locked: False
-    points: []
-    colors: []
-    text: "wrist_top"
-    mesh_resource: ''
-    mesh_use_embedded_materials: False
-  -
-    header:
-      seq: 0
-      stamp:
-        secs: 1651003161
-        nsecs: 766932964
-      frame_id: "/camera_color_optical_frame"
-    ns: ''
-    id: 134
-    type: 1
-    action: 0
-    pose:
-      position:
-        x: 0.100040371589
-        y: -0.0892338916474
-        z: 0.971143314039
-      orientation:
-        x: 0.999737501831
-        y: -0.00162452511204
-        z: 0.00672076359396
-        w: -0.0218430695578
-    scale:
-      x: 0.0314
-      y: 0.0314
-      z: 0.005
-    color:
-      r: 0.0
-      g: 1.0
-      b: 0.564705908298
-      a: 0.330000013113
-    lifetime:
-      secs: 0
-      nsecs: 200000000
-    frame_locked: False
-    points: []
-    colors: []
-    text: "shoulder_top"
-    mesh_resource: ''
-    mesh_use_embedded_materials: False

stretch_camera_testrig/data/.ipynb_checkpoints/Testrig_Data_Analyse-checkpoint.ipynb

@@ -2,7 +2,7 @@
  "cells": [
   {
    "cell_type": "code",
-   "execution_count": 36,
+   "execution_count": 12,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -13,16 +13,16 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 37,
+   "execution_count": 13,
    "metadata": {},
    "outputs": [],
    "source": [
-    "filename = 'testrig_collected_data_202204261559.yaml'"
+    "filename = 'testrig_collected_data_202204271357.yaml'"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 53,
+   "execution_count": 17,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -53,12 +53,16 @@
     "                null_cnt[key] = null_cnt[key] + 1\n",
     "    for key in data_keys:\n",
     "        avg_pose_sums[key] = avg_pose_sums[key]/(len(data)-null_cnt[key])\n",
-    "    return avg_pose_sums"
+    "    return avg_pose_sums\n",
+    "\n",
+    "def euclidean_error(r1, r2):\n",
+    "    dist = np.linalg.norm(r1[:3, 3] - r2[:3, 3])\n",
+    "    return float(dist)"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 54,
+   "execution_count": 18,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -67,20 +71,22 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 55,
+   "execution_count": 19,
    "metadata": {},
    "outputs": [
     {
-     "ename": "TypeError",
-     "evalue": "unsupported operand type(s) for -: 'int' and 'dict'",
-     "output_type": "error",
-     "traceback": [
-      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
-      "\u001b[0;31mTypeError\u001b[0m                                 Traceback (most recent call last)",
-      "\u001b[0;32m<ipython-input-55-42cc4761766e>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m()\u001b[0m\n\u001b[0;32m----> 1\u001b[0;31m \u001b[0mget_avg_pos_vals\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mdata\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m",
-      "\u001b[0;32m<ipython-input-53-b02bf3890354>\u001b[0m in \u001b[0;36mget_avg_pos_vals\u001b[0;34m(data)\u001b[0m\n\u001b[1;32m     25\u001b[0m                 \u001b[0mnull_cnt\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mkey\u001b[0m\u001b[0;34m]\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mnull_cnt\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mkey\u001b[0m\u001b[0;34m]\u001b[0m \u001b[0;34m+\u001b[0m \u001b[0;36m1\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     26\u001b[0m     \u001b[0;32mfor\u001b[0m \u001b[0mkey\u001b[0m \u001b[0;32min\u001b[0m \u001b[0mdata_keys\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 27\u001b[0;31m         \u001b[0mavg_pose_sums\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mkey\u001b[0m\u001b[0;34m]\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mavg_pose_sums\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mkey\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m/\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mlen\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mdata\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m-\u001b[0m\u001b[0mnull_cnt\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m     28\u001b[0m     \u001b[0;32mreturn\u001b[0m \u001b[0mavg_pose_sums\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
-      "\u001b[0;31mTypeError\u001b[0m: unsupported operand type(s) for -: 'int' and 'dict'"
-     ]
+     "data": {
+      "text/plain": [
+       "{'base_left_marker_pose': array([ 0.01560918, -0.03676896,  0.97180282]),\n",
+       " 'base_right_marker_pose': array([ 0.08429622, -0.03563762,  0.9740998 ]),\n",
+       " 'shoulder_marker_pose': array([ 0.09266814, -0.09151441,  0.97548074]),\n",
+       " 'wrist_inside_marker_pose': array([ 0.00508762, -0.09562531,  0.97365567]),\n",
+       " 'wrist_top_marker_pose': array([ 0.0466689 , -0.09545316,  0.97414828])}"
+      ]
+     },
+     "execution_count": 19,
+     "metadata": {},
+     "output_type": "execute_result"
     }
    ],
    "source": [
@@ -89,7 +95,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 30,
+   "execution_count": 20,
    "metadata": {},
    "outputs": [
     {
@@ -103,7 +109,7 @@
        " 'base_left_marker_pose']"
       ]
      },
-     "execution_count": 30,
+     "execution_count": 20,
      "metadata": {},
      "output_type": "execute_result"
     }
@@ -114,19 +120,19 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 31,
+   "execution_count": 21,
    "metadata": {},
    "outputs": [
     {
      "data": {
       "text/plain": [
-       "array([[ 0.99989452, -0.01255395, -0.00730402,  0.0239161 ],\n",
-       "       [-0.01277629, -0.99943037, -0.03123614, -0.03452451],\n",
-       "       [-0.00690772,  0.03132616, -0.99948535,  0.96807451],\n",
+       "array([[ 0.99847575, -0.00292909, -0.05511444,  0.01565563],\n",
+       "       [-0.00900108, -0.99386336, -0.11024792, -0.03676937],\n",
+       "       [-0.05445329,  0.11057596, -0.99237483,  0.97191084],\n",
        "       [ 0.        ,  0.        ,  0.        ,  1.        ]])"
       ]
      },
-     "execution_count": 31,
+     "execution_count": 21,
      "metadata": {},
      "output_type": "execute_result"
     }
@@ -138,63 +144,31 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 32,
-   "metadata": {},
-   "outputs": [
-    {
-     "data": {
-      "text/plain": [
-       "array([ 0.0239161 , -0.03452451,  0.96807451])"
-      ]
-     },
-     "execution_count": 32,
-     "metadata": {},
-     "output_type": "execute_result"
-    }
-   ],
-   "source": [
-    "m = np.array(m)[:3,3]\n",
-    "m"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 33,
+   "execution_count": null,
    "metadata": {},
-   "outputs": [
-    {
-     "data": {
-      "text/plain": [
-       "(3,)"
-      ]
-     },
-     "execution_count": 33,
-     "metadata": {},
-     "output_type": "execute_result"
-    }
-   ],
+   "outputs": [],
    "source": [
-    "m.shape"
+    "nominal_poses = get_dict()"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 35,
+   "execution_count": 22,
    "metadata": {},
    "outputs": [
     {
      "data": {
       "text/plain": [
-       "(3,)"
+       "0.0"
       ]
      },
-     "execution_count": 35,
+     "execution_count": 22,
      "metadata": {},
      "output_type": "execute_result"
     }
    ],
    "source": [
-    "np.zeros(3).shape"
+    "euclidean_error(m,m)"
    ]
   },
   {

stretch_camera_testrig/data/Testrig_Data_Analyse.ipynb → stretch_camera_testrig/data/Testrig_Data_Analyse_rough.ipynb

@@ -2,27 +2,28 @@
  "cells": [
   {
    "cell_type": "code",
-   "execution_count": 4,
+   "execution_count": 30,
    "metadata": {},
    "outputs": [],
    "source": [
     "import yaml\n",
     "import numpy as np\n",
-    "import pandas as pd"
+    "import pandas as pd\n",
+    "import math"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 5,
+   "execution_count": 13,
    "metadata": {},
    "outputs": [],
    "source": [
-    "filename = 'testrig_collected_data_202204261813.yaml'"
+    "filename = 'testrig_collected_data_202204271357.yaml'"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 6,
+   "execution_count": 49,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -53,12 +54,20 @@
     "                null_cnt[key] = null_cnt[key] + 1\n",
     "    for key in data_keys:\n",
     "        avg_pose_sums[key] = avg_pose_sums[key]/(len(data)-null_cnt[key])\n",
-    "    return avg_pose_sums"
+    "    return avg_pose_sums\n",
+    "\n",
+    "def euclidean_error(r1, r2):\n",
+    "    p1 = r1[:3, 3]\n",
+    "    p2 = r2[:3, 3]\n",
+    "    dist1 = math.sqrt(math.pow(p1[0],2) + math.pow(p1[1],2) + math.pow(p1[2],2))\n",
+    "    dist2 = math.sqrt(math.pow(p2[0],2) + math.pow(p2[1],2) + math.pow(p2[2],2))\n",
+    "#     dist = np.linalg.norm(r1[:3, 3] - r2[:3, 3])\n",
+    "    return float(abs(dist1-dist2))"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 7,
+   "execution_count": 50,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -67,20 +76,20 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 8,
+   "execution_count": 51,
    "metadata": {},
    "outputs": [
     {
      "data": {
       "text/plain": [
-       "{'base_left_marker_pose': array([ 0.0238681 , -0.03427565,  0.96917111]),\n",
-       " 'base_right_marker_pose': array([ 0.09188251, -0.03348409,  0.96873629]),\n",
-       " 'shoulder_marker_pose': array([ 0.09998645, -0.08940415,  0.97319534]),\n",
-       " 'wrist_inside_marker_pose': array([ 0.01273042, -0.09285669,  0.97215891]),\n",
-       " 'wrist_top_marker_pose': array([ 0.05442445, -0.09320034,  0.97401416])}"
+       "{'base_left_marker_pose': array([ 0.01560918, -0.03676896,  0.97180282]),\n",
+       " 'base_right_marker_pose': array([ 0.08429622, -0.03563762,  0.9740998 ]),\n",
+       " 'shoulder_marker_pose': array([ 0.09266814, -0.09151441,  0.97548074]),\n",
+       " 'wrist_inside_marker_pose': array([ 0.00508762, -0.09562531,  0.97365567]),\n",
+       " 'wrist_top_marker_pose': array([ 0.0466689 , -0.09545316,  0.97414828])}"
       ]
      },
-     "execution_count": 8,
+     "execution_count": 51,
      "metadata": {},
      "output_type": "execute_result"
     }
@@ -91,7 +100,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 30,
+   "execution_count": 52,
    "metadata": {},
    "outputs": [
     {
@@ -105,7 +114,7 @@
        " 'base_left_marker_pose']"
       ]
      },
-     "execution_count": 30,
+     "execution_count": 52,
      "metadata": {},
      "output_type": "execute_result"
     }
@@ -116,19 +125,19 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 31,
+   "execution_count": 53,
    "metadata": {},
    "outputs": [
     {
      "data": {
       "text/plain": [
-       "array([[ 0.99989452, -0.01255395, -0.00730402,  0.0239161 ],\n",
-       "       [-0.01277629, -0.99943037, -0.03123614, -0.03452451],\n",
-       "       [-0.00690772,  0.03132616, -0.99948535,  0.96807451],\n",
+       "array([[ 0.99847575, -0.00292909, -0.05511444,  0.01565563],\n",
+       "       [-0.00900108, -0.99386336, -0.11024792, -0.03676937],\n",
+       "       [-0.05445329,  0.11057596, -0.99237483,  0.97191084],\n",
        "       [ 0.        ,  0.        ,  0.        ,  1.        ]])"
       ]
      },
-     "execution_count": 31,
+     "execution_count": 53,
      "metadata": {},
      "output_type": "execute_result"
     }
@@ -140,63 +149,56 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 32,
+   "execution_count": 54,
    "metadata": {},
-   "outputs": [
-    {
-     "data": {
-      "text/plain": [
-       "array([ 0.0239161 , -0.03452451,  0.96807451])"
-      ]
-     },
-     "execution_count": 32,
-     "metadata": {},
-     "output_type": "execute_result"
-    }
-   ],
+   "outputs": [],
    "source": [
-    "m = np.array(m)[:3,3]\n",
-    "m"
+    "nominal_poses = get_dict('testrig_marker_info.yaml')"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 33,
+   "execution_count": 55,
    "metadata": {},
    "outputs": [
     {
      "data": {
       "text/plain": [
-       "(3,)"
+       "array([[ 1.     ,  0.     ,  0.     ,  0.009  ],\n",
+       "       [ 0.     , -1.     ,  0.     , -0.06418],\n",
+       "       [ 0.     ,  0.     , -1.     ,  0.97551],\n",
+       "       [ 0.     ,  0.     ,  0.     ,  1.     ]])"
       ]
      },
-     "execution_count": 33,
+     "execution_count": 55,
      "metadata": {},
      "output_type": "execute_result"
     }
    ],
    "source": [
-    "m.shape"
+    "mn = nominal_poses['testrig_aruco_marker_info'][data_keys[0]]\n",
+    "mn = np.array(mn)\n",
+    "mn"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 35,
+   "execution_count": 56,
    "metadata": {},
    "outputs": [
     {
      "data": {
       "text/plain": [
-       "(3,)"
+       "0.004928270424978343"
       ]
      },
-     "execution_count": 35,
+     "execution_count": 56,
      "metadata": {},
      "output_type": "execute_result"
     }
    ],
    "source": [
-    "np.zeros(3).shape"
+    "euclidean_error(m,mn)"
    ]
   },
   {

stretch_camera_testrig/data/results/testrig_results_134322070297_202204261813.yaml

@@ -1,68 +0,0 @@
-Realsense Details:
-  firmware: 05.13.00.50
-  serial: '134322070297'
-  usb: '3.2'
-capture_id: '202204261813'
-lighting_condition:
-  brightness: null
-  temperature: null
-null_frames:
-  base_left_marker_pose: 272
-  base_right_marker_pose: 192
-  shoulder_marker_pose: 0
-  wrist_inside_marker_pose: 0
-  wrist_top_marker_pose: 0
-number_samples: 600
-performance_metrics:
-  angle_rotation_error:
-    base_left_marker_pose:
-      maximum: 3.1258827310374016
-      mean: 3.0725028300116475
-      median: 3.0746407646264298
-      rmse: 3.0725811678416126
-    base_right_marker_pose:
-      maximum: 3.1415600463725744
-      mean: 3.1281164385598323
-      median: 3.131120104508878
-      rmse: 3.1281361865272763
-    shoulder_marker_pose:
-      maximum: 3.1389336379556836
-      mean: 3.0512989442373804
-      median: 3.0493995194523613
-      rmse: 3.0515383390778847
-    wrist_inside_marker_pose:
-      maximum: 3.13906352088329
-      mean: 2.9671124450790147
-      median: 2.9631123705678677
-      rmse: 2.967834290736253
-    wrist_top_marker_pose:
-      maximum: 3.1415549339204576
-      mean: 3.0767871487676173
-      median: 3.084675730671946
-      rmse: 3.07717660415396
-  euclidean_error:
-    base_left_marker_pose:
-      maximum: 0.03414611432137011
-      mean: 0.03399378330856081
-      median: 0.03399530706887118
-      rmse: 0.033993842471503145
-    base_right_marker_pose:
-      maximum: 0.03455412890846682
-      mean: 0.03436417794040963
-      median: 0.03436250360749031
-      rmse: 0.03436421681807256
-    shoulder_marker_pose:
-      maximum: 0.0345151353685676
-      mean: 0.03424525455350311
-      median: 0.0342402193054973
-      rmse: 0.03424533774691318
-    wrist_inside_marker_pose:
-      maximum: 0.03582231291904326
-      mean: 0.035335379096608645
-      median: 0.03532788514175855
-      rmse: 0.03533579402749221
-    wrist_top_marker_pose:
-      maximum: 0.03533980379142684
-      mean: 0.034731985234355375
-      median: 0.034705777990379355
-      rmse: 0.03473224997401931

stretch_camera_testrig/data/results/testrig_results_134322070297_202204261845.yaml

@@ -1,68 +0,0 @@
-Realsense Details:
-  firmware: 05.13.00.50
-  serial: '134322070297'
-  usb: '3.2'
-capture_id: '202204261845'
-lighting_condition:
-  brightness: null
-  temperature: null
-null_frames:
-  base_left_marker_pose: 0
-  base_right_marker_pose: 61
-  shoulder_marker_pose: 131
-  wrist_inside_marker_pose: 0
-  wrist_top_marker_pose: 0
-number_samples: 600
-performance_metrics:
-  angle_rotation_error:
-    base_left_marker_pose:
-      maximum: 0.41294171847747907
-      mean: 0.21592936261674547
-      median: 0.22412447149939774
-      rmse: 0.24310818842633086
-    base_right_marker_pose:
-      maximum: 0.15919860191633364
-      mean: 0.05750833432180123
-      median: 0.05552143849311667
-      rmse: 0.061931342858066916
-    shoulder_marker_pose:
-      maximum: 0.3864074601579912
-      mean: 0.1276920001965476
-      median: 0.12322134360935724
-      rmse: 0.141470133793462
-    wrist_inside_marker_pose:
-      maximum: 0.5790925519286533
-      mean: 0.16825776269524945
-      median: 0.15438178417289702
-      rmse: 0.18893775669030632
-    wrist_top_marker_pose:
-      maximum: 0.40511702679212597
-      mean: 0.14459008904920764
-      median: 0.145501249651101
-      rmse: 0.15889458853774657
-  euclidean_error:
-    base_left_marker_pose:
-      maximum: 0.03401788769999435
-      mean: 0.03335710878766747
-      median: 0.033229176839096174
-      rmse: 0.03335805300035855
-    base_right_marker_pose:
-      maximum: 0.034748772059683325
-      mean: 0.03416716556930868
-      median: 0.03419650881056504
-      rmse: 0.03416792013693636
-    shoulder_marker_pose:
-      maximum: 0.034352006936188964
-      mean: 0.034017375298520065
-      median: 0.03401204867261022
-      rmse: 0.034017477444385574
-    wrist_inside_marker_pose:
-      maximum: 0.03598241881671745
-      mean: 0.035063922182315986
-      median: 0.035045264256581214
-      rmse: 0.03506478866265719
-    wrist_top_marker_pose:
-      maximum: 0.03549062723316463
-      mean: 0.03480551427832518
-      median: 0.03478254826498238
-      rmse: 0.03480618391418952

stretch_camera_testrig/data/results/testrig_results_134322070297_202204271550.yaml

@@ -0,0 +1,67 @@
+Realsense Details:
+  firmware: 05.13.00.50
+  serial: '134322070297'
+  usb: '3.2'
+capture_id: '202204271550'
+lighting_condition:
+  illuminance: null
+null_frames:
+  base_left_marker_pose: 0
+  base_right_marker_pose: 135
+  shoulder_marker_pose: 3
+  wrist_inside_marker_pose: 2
+  wrist_top_marker_pose: 0
+number_samples: 600
+performance_metrics:
+  angle_rotation:
+    base_left_marker_pose:
+      maximum: 4.766804002899655
+      mean: 2.3990214871891156
+      median: 2.359023484905266
+      rmse: 2.5087286549543864
+    base_right_marker_pose:
+      maximum: 5.005811064362725
+      mean: 1.748539203063977
+      median: 1.6631606620070551
+      rmse: 1.9355421739709848
+    shoulder_marker_pose:
+      maximum: 12.088164512200914
+      mean: 6.821694971153336
+      median: 6.836096110534366
+      rmse: 7.090196479520287
+    wrist_inside_marker_pose:
+      maximum: 16.670845318858674
+      mean: 8.265394579026248
+      median: 8.198434813676382
+      rmse: 8.695479433952881
+    wrist_top_marker_pose:
+      maximum: 19.99405467497106
+      mean: 6.9158414464690185
+      median: 6.616562200716092
+      rmse: 7.787156964716686
+  euclidean_error:
+    base_left_marker_pose:
+      maximum: 0.009057293197401117
+      mean: 0.008391050724211464
+      median: 0.008386940683026423
+      rmse: 0.008392687194295404
+    base_right_marker_pose:
+      maximum: 0.009107418766137632
+      mean: 0.008574379652446798
+      median: 0.008571480261610698
+      rmse: 0.008576032714184372
+    shoulder_marker_pose:
+      maximum: 0.008957196011541102
+      mean: 0.007077670461131529
+      median: 0.007091959781101775
+      rmse: 0.007108683689396519
+    wrist_inside_marker_pose:
+      maximum: 0.009307531998013263
+      mean: 0.008250919662536292
+      median: 0.008370306065720645
+      rmse: 0.008266255932579248
+    wrist_top_marker_pose:
+      maximum: 0.010189692502520331
+      mean: 0.00916283104270397
+      median: 0.009249489231716201
+      rmse: 0.009175624550455497

stretch_camera_testrig/data/results/testrig_results_202202171855.yaml

@@ -1,65 +0,0 @@
-capture_id: '202202171855'
-lighting_condition:
-  brightness: null
-  temperature: null
-null_frames:
-  base_left_marker_pose: 0
-  base_right_marker_pose: 0
-  shoulder_marker_pose: 327
-  wrist_inside_marker_pose: 0
-  wrist_top_marker_pose: 0
-number_samples: 600
-performance_metrics:
-  angle_rotation_error:
-    base_left_marker_pose:
-      maximum: 3.130310772767618
-      mean: 3.1123105605302857
-      median: 3.1121101802740316
-      rmse: 3.1123183433291164
-    base_right_marker_pose:
-      maximum: 3.141490159715747
-      mean: 3.126268687591132
-      median: 3.1270113289070975
-      rmse: 3.1262809684459114
-    shoulder_marker_pose:
-      maximum: 3.1351679171566076
-      mean: 3.112359136964902
-      median: 3.1132823943622094
-      rmse: 3.112370862795674
-    wrist_inside_marker_pose:
-      maximum: 3.1351074913380303
-      mean: 3.091203628929577
-      median: 3.0897949788692305
-      rmse: 3.09122966284615
-    wrist_top_marker_pose:
-      maximum: 3.1412404426476863
-      mean: 3.1094737772610457
-      median: 3.1103044636557806
-      rmse: 3.1095094548879647
-  euclidean_error:
-    base_left_marker_pose:
-      maximum: 0.5824225026965391
-      mean: 0.5820963567556977
-      median: 0.5820901959143456
-      rmse: 0.5820963644996675
-    base_right_marker_pose:
-      maximum: 0.5793795286072739
-      mean: 0.578940614355908
-      median: 0.5789297846748012
-      rmse: 0.5789406328075614
-    shoulder_marker_pose:
-      maximum: 0.5814463353933174
-      mean: 0.5812017213971998
-      median: 0.5812040960870808
-      rmse: 0.5812017272530052
-    wrist_inside_marker_pose:
-      maximum: 0.5788094647317441
-      mean: 0.5784990979155296
-      median: 0.5784922563334053
-      rmse: 0.5784991059700069
-    wrist_top_marker_pose:
-      maximum: 0.5806082586912994
-      mean: 0.5802254663635843
-      median: 0.5802384545676887
-      rmse: 0.5802254782763624
-realsense_firmware: 05.13.00.50

stretch_camera_testrig/data/results/testrig_results_202203081051.yaml

@@ -1,65 +0,0 @@
-capture_id: '202203081051'
-lighting_condition:
-  brightness: null
-  temperature: null
-null_frames:
-  base_left_marker_pose: 600
-  base_right_marker_pose: 600
-  shoulder_marker_pose: 0
-  wrist_inside_marker_pose: 0
-  wrist_top_marker_pose: 0
-number_samples: 600
-performance_metrics:
-  angle_rotation_error:
-    base_left_marker_pose:
-      maximum: .nan
-      mean: .nan
-      median: .nan
-      rmse: .nan
-    base_right_marker_pose:
-      maximum: .nan
-      mean: .nan
-      median: .nan
-      rmse: .nan
-    shoulder_marker_pose:
-      maximum: 3.097955498135791
-      mean: 3.073899037378878
-      median: 3.0741315861747056
-      rmse: 3.0739089414710428
-    wrist_inside_marker_pose:
-      maximum: 3.1415761356878966
-      mean: 3.132097089689872
-      median: 3.1341885277231425
-      rmse: 3.1321072207088934
-    wrist_top_marker_pose:
-      maximum: 3.1415892941462182
-      mean: 3.130561913781429
-      median: 3.1323310066834207
-      rmse: 3.130571838773318
-  euclidean_error:
-    base_left_marker_pose:
-      maximum: .nan
-      mean: .nan
-      median: .nan
-      rmse: .nan
-    base_right_marker_pose:
-      maximum: .nan
-      mean: .nan
-      median: .nan
-      rmse: .nan
-    shoulder_marker_pose:
-      maximum: 0.382899354953261
-      mean: 0.3825723337040798
-      median: 0.3825632955437427
-      rmse: 0.38257234197690604
-    wrist_inside_marker_pose:
-      maximum: 0.3790289222347782
-      mean: 0.3788044075180077
-      median: 0.37881162813600683
-      rmse: 0.3788044182591934
-    wrist_top_marker_pose:
-      maximum: 0.3815277974107985
-      mean: 0.38131954873736296
-      median: 0.3813172708681125
-      rmse: 0.3813195561084727
-realsense_details: ''

stretch_camera_testrig/data/results/testrig_results_202203081401.yaml

@@ -1,68 +0,0 @@
-capture_id: '202203081401'
-lighting_condition:
-  brightness: null
-  temperature: null
-null_frames:
-  base_left_marker_pose: 600
-  base_right_marker_pose: 600
-  shoulder_marker_pose: 0
-  wrist_inside_marker_pose: 0
-  wrist_top_marker_pose: 0
-number_samples: 600
-performance_metrics:
-  angle_rotation_error:
-    base_left_marker_pose:
-      maximum: .nan
-      mean: .nan
-      median: .nan
-      rmse: .nan
-    base_right_marker_pose:
-      maximum: .nan
-      mean: .nan
-      median: .nan
-      rmse: .nan
-    shoulder_marker_pose:
-      maximum: 3.141584519643849
-      mean: 3.126448207110162
-      median: 3.1276875522310275
-      rmse: 3.1264639715464964
-    wrist_inside_marker_pose:
-      maximum: 3.141569730682565
-      mean: 3.1315863064453504
-      median: 3.1329069411440544
-      rmse: 3.131594936497842
-    wrist_top_marker_pose:
-      maximum: 3.141142764422492
-      mean: 3.1230911150059053
-      median: 3.1232765976473784
-      rmse: 3.1230998350351697
-  euclidean_error:
-    base_left_marker_pose:
-      maximum: .nan
-      mean: .nan
-      median: .nan
-      rmse: .nan
-    base_right_marker_pose:
-      maximum: .nan
-      mean: .nan
-      median: .nan
-      rmse: .nan
-    shoulder_marker_pose:
-      maximum: 0.46059878545645616
-      mean: 0.46029296570007244
-      median: 0.46030180067131776
-      rmse: 0.4602929833290485
-    wrist_inside_marker_pose:
-      maximum: 0.46751724743962775
-      mean: 0.4673098171295163
-      median: 0.4673148698265675
-      rmse: 0.4673098241295182
-    wrist_top_marker_pose:
-      maximum: 0.47689815540831476
-      mean: 0.4767455667689777
-      median: 0.4767559937236129
-      rmse: 0.4767455718786323
-realsense_details:
-  firmware: 05.13.00.50
-  serial: '134322074171'
-  usb: '3.2'

stretch_camera_testrig/data/results/testrig_results_202203091216.yaml

@@ -1,68 +0,0 @@
-Realsense Details:
-  firmware: 05.13.00.50
-  serial: 040322072690
-  usb: '3.2'
-capture_id: '202203091216'
-lighting_condition:
-  brightness: null
-  temperature: null
-null_frames:
-  base_left_marker_pose: 0
-  base_right_marker_pose: 0
-  shoulder_marker_pose: 0
-  wrist_inside_marker_pose: 0
-  wrist_top_marker_pose: 0
-number_samples: 600
-performance_metrics:
-  angle_rotation_error:
-    base_left_marker_pose:
-      maximum: 3.1258228069143574
-      mean: 3.090495677559066
-      median: 3.0898352426275784
-      rmse: 3.090518944468393
-    base_right_marker_pose:
-      maximum: 3.141583905313619
-      mean: 3.13170178989854
-      median: 3.1330645842886318
-      rmse: 3.131710155183963
-    shoulder_marker_pose:
-      maximum: 3.117145749446174
-      mean: 3.07786045902488
-      median: 3.0775851890028987
-      rmse: 3.077881724110398
-    wrist_inside_marker_pose:
-      maximum: 3.141327402355638
-      mean: 3.1058216452255563
-      median: 3.1062270425421623
-      rmse: 3.1058908098530496
-    wrist_top_marker_pose:
-      maximum: 3.141473924121154
-      mean: 3.1082500276239067
-      median: 3.112768283406675
-      rmse: 3.1083324817414675
-  euclidean_error:
-    base_left_marker_pose:
-      maximum: 0.5790332362367818
-      mean: 0.5786669509096053
-      median: 0.5786675250115955
-      rmse: 0.5786669620339555
-    base_right_marker_pose:
-      maximum: 0.5775522418555454
-      mean: 0.5768582518676268
-      median: 0.5768797683076176
-      rmse: 0.5768583085342299
-    shoulder_marker_pose:
-      maximum: 0.5841755924455108
-      mean: 0.583766495075335
-      median: 0.5837691236193006
-      rmse: 0.5837665124825672
-    wrist_inside_marker_pose:
-      maximum: 0.5809248639389007
-      mean: 0.5804067895137061
-      median: 0.5804198165198842
-      rmse: 0.5804068348609547
-    wrist_top_marker_pose:
-      maximum: 0.5824827029263467
-      mean: 0.5817229397175923
-      median: 0.5817343124317399
-      rmse: 0.5817230226763274

stretch_camera_testrig/data/results/testrig_results_202204221727.yaml

@@ -1,68 +0,0 @@
-Realsense Details:
-  firmware: 05.13.00.50
-  serial: '134322070297'
-  usb: '3.2'
-capture_id: '202204221727'
-lighting_condition:
-  brightness: null
-  temperature: null
-null_frames:
-  base_left_marker_pose: 45
-  base_right_marker_pose: 0
-  shoulder_marker_pose: 0
-  wrist_inside_marker_pose: 0
-  wrist_top_marker_pose: 0
-number_samples: 600
-performance_metrics:
-  angle_rotation_error:
-    base_left_marker_pose:
-      maximum: .nan
-      mean: .nan
-      median: .nan
-      rmse: .nan
-    base_right_marker_pose:
-      maximum: 3.1415102398810113
-      mean: 3.123058951288247
-      median: 3.124318210965746
-      rmse: 3.1230811369254297
-    shoulder_marker_pose:
-      maximum: 3.1335815142187062
-      mean: 3.083481179422443
-      median: 3.0833914703359806
-      rmse: 3.0835367100259767
-    wrist_inside_marker_pose:
-      maximum: .nan
-      mean: .nan
-      median: .nan
-      rmse: .nan
-    wrist_top_marker_pose:
-      maximum: .nan
-      mean: .nan
-      median: .nan
-      rmse: .nan
-  euclidean_error:
-    base_left_marker_pose:
-      maximum: 0.915788144606525
-      mean: 0.9154234022733942
-      median: 0.9154204292171265
-      rmse: 0.9154234113171679
-    base_right_marker_pose:
-      maximum: 0.9214418418458941
-      mean: 0.920910422416328
-      median: 0.9209082137309761
-      rmse: 0.9209104385493279
-    shoulder_marker_pose:
-      maximum: 0.9262525606179585
-      mean: 0.9255763067776382
-      median: 0.9255706057771111
-      rmse: 0.9255763231902148
-    wrist_inside_marker_pose:
-      maximum: 0.9306561678487971
-      mean: 0.9299437368304545
-      median: 0.9299687248825842
-      rmse: 0.9299437768626291
-    wrist_top_marker_pose:
-      maximum: 0.934151074834107
-      mean: 0.9339303855491827
-      median: 0.9339337648183319
-      rmse: 0.9339303887654067

stretch_camera_testrig/data/results/testrig_results_202204261414.yaml

@@ -1,68 +0,0 @@
-Realsense Details:
-  firmware: 05.13.00.50
-  serial: '134322070297'
-  usb: '3.2'
-capture_id: '202204261414'
-lighting_condition:
-  brightness: null
-  temperature: null
-null_frames:
-  base_left_marker_pose: 59
-  base_right_marker_pose: 90
-  shoulder_marker_pose: 4
-  wrist_inside_marker_pose: 0
-  wrist_top_marker_pose: 0
-number_samples: 600
-performance_metrics:
-  angle_rotation_error:
-    base_left_marker_pose:
-      maximum: 3.1275576076978107
-      mean: 3.0844446192804003
-      median: 3.085289002989193
-      rmse: 3.084509116150328
-    base_right_marker_pose:
-      maximum: 3.1415675780257146
-      mean: 3.120280746109533
-      median: 3.1220539908722342
-      rmse: 3.1203188088447393
-    shoulder_marker_pose:
-      maximum: 3.1412206574486286
-      mean: 3.0803388462890138
-      median: 3.086926562084815
-      rmse: 3.0805747860699184
-    wrist_inside_marker_pose:
-      maximum: 3.140033572342562
-      mean: 3.028636293238632
-      median: 3.034294458026425
-      rmse: 3.0294976506020994
-    wrist_top_marker_pose:
-      maximum: 3.1414217232795028
-      mean: 2.9309349672425977
-      median: 2.9226252953142797
-      rmse: 2.9319990113675507
-  euclidean_error:
-    base_left_marker_pose:
-      maximum: 0.9724175395587147
-      mean: 0.9711948725610475
-      median: 0.9711874765852018
-      rmse: 0.9711949482590717
-    base_right_marker_pose:
-      maximum: 0.9739097812523311
-      mean: 0.9731779719411534
-      median: 0.9731709697130762
-      rmse: 0.973177992657312
-    shoulder_marker_pose:
-      maximum: 0.9860701882814852
-      mean: 0.9846072444944528
-      median: 0.9846312727709248
-      rmse: 0.9846073629903658
-    wrist_inside_marker_pose:
-      maximum: 0.9834809515322587
-      mean: 0.9805508942375426
-      median: 0.9805141568620914
-      rmse: 0.9805512942588581
-    wrist_top_marker_pose:
-      maximum: 0.9819304933010518
-      mean: 0.9784622428409652
-      median: 0.9783632048498961
-      rmse: 0.9784627071336145

stretch_camera_testrig/nodes/collect_head_calibration_data.py

@@ -95,11 +95,7 @@ class CollectHeadCalibrationDataNode:
                     self.shoulder_marker_pose = marker.pose
 
     def calibration_data_callback_testrig(self, marker_array, accel):
-        # Prepare and assign data for calibration to member
-        # variables. The configuration of the robot's joints, the
-        # D435i acceleration, and the 3D fiducial estimates made via
-        # D435i imagery are expected to be synchronized for this
-        # callback.
+
         with self.data_lock:
             self.wrist_inside_marker_pose = None
             self.wrist_top_marker_pose = None

stretch_camera_testrig/nodes/testrig_analyze_data.py

@@ -44,7 +44,11 @@ class TestRig_Analyze:
         if data_filename:
             self.data_filename = data_filename
         else:
-            files = os.listdir(self.data_directory)
+            all_files = os.listdir(self.data_directory)
+            files = []
+            for f in all_files:
+                if f.startswith("testrig_collected_data_"):
+                    files.append(f)
             files.sort()
             if files[-1].startswith("testrig_collected_data_"):
                 self.data_filename = self.data_directory + '/' + files[-1]
@@ -58,8 +62,7 @@ class TestRig_Analyze:
                                   'number_samples': None,
                                   'null_frames': {},
                                   'lighting_condition': {
-                                      'temperature': None,
-                                      'brightness': None,
+                                      'illuminance': None
                                   },
                                   'performance_metrics': {},
                                   'Realsense Details': self.realsense_details
@@ -94,7 +97,7 @@ class TestRig_Analyze:
         return rn_vals
 
     def populate_performance_metrics(self):
-        self.test_results_dict['performance_metrics']['angle_rotation_error'] = self.get_performance_metric(
+        self.test_results_dict['performance_metrics']['angle_rotation'] = self.get_performance_metric(
             self.angle_rotation_error_dict)
         self.test_results_dict['performance_metrics']['euclidean_error'] = self.get_performance_metric(
             self.euclidean_error_dict)
@@ -189,20 +192,27 @@ class TestRig_Analyze:
         for key in error_dict.keys():
             for i in range(Num_samples):
                 if type(data_dict[key][i]) != type(None):
-                    error_dict[key].append(self.angle_rot_error(nominal_poses_dict[key], data_dict[key][i]))
+                    error_dict[key].append(self.angle_rot(nominal_poses_dict[key], data_dict[key][i]))
                 else:
                     error_dict[key].append(None)
         return error_dict
 
-    def angle_rot_error(self, r1, r2):
+    def angle_rot(self, r1, r2):
         rot1 = np.array(r1)[:3, :3]
         rot2 = np.array(r2)[:3, :3]
         rot12 = np.matmul(rot1.T, rot2)
         theta_error = np.arccos((np.trace(rot12) - 1) / 2)
+        # Converting to Degrees
+        theta_error = np.degrees(theta_error)
         return float(theta_error)
 
     def euclidean_error(self, r1, r2):
-        dist = np.linalg.norm(r1[:3, 3] - r2[:3, 3])
+        p1 = r1[:3, 3]
+        p2 = r2[:3, 3]
+        dist1 = np.linalg.norm(p1)
+        dist2 = np.linalg.norm(p2)
+        dist = abs(dist1 - dist2)
+        #     dist = np.linalg.norm(p1-p2)
         return float(dist)
 
     def get_nominal_pose_dict(self, filename):
@@ -225,7 +235,7 @@ class TestRig_Analyze:
             error = {'error_data': {}}
             for key in self.data_keys:
                 vals = {}
-                vals['angle_rotation_error'] = self.angle_rotation_error_dict[key][i]
+                vals['angle_rotation'] = self.angle_rotation_error_dict[key][i]
                 vals['euclidean_error'] = self.euclidean_error_dict[key][i]
                 error['error_data'][key] = vals
             testrig_results.append(error)

stretch_camera_testrig/nodes/testrig_collect_data

@@ -9,5 +9,6 @@ if __name__ == '__main__':
     try:
         node = CollectHeadCalibrationDataNode(test_rig=True)
         node.main()
+        rospy.signal_shutdown('Finished Test Data Collection.')
     except rospy.ROSInterruptException:
         pass

stretch_camera_testrig/nodes/testrig_dashboard.py

@@ -23,10 +23,10 @@ class TestRig_dashboard():
                           'wrist_top_marker_pose',
                           'shoulder_marker_pose']
         self.metrics_keys = ["maximum", "mean", "median", "rmse"]
-        self.error_keys = ["angle_rotation_error", "euclidean_error"]
+        self.error_keys = ["angle_rotation", "euclidean_error"]
         self.window = Tk()
         self.window.title("Camera Test rig Dashboard")
-        self.window.geometry('700x900')
+        self.window.geometry('800x900')
 
         self.data_file_name = None
         self.test_rig = None
@@ -96,12 +96,12 @@ class TestRig_dashboard():
                                        font=("Arial", 14))
         self.save_results_btn.place(x=x_off_bottom + 150, y=y_off_bottom + 500)
 
-        self.run_new_test_btn = Button(self.window, text="Run a new Test", command=self.run_new_test,
+        self.run_new_test_btn = Button(self.window, text="Run a new Test", command=self.run_new_test,state='disabled',
                                        font=("Arial", 14))
         self.run_new_test_btn.place(x=x_off_bottom + 150, y=y_off_bottom + 540)
 
         self.target_samples = IntVar()
-        self.target_samples_entry = Entry(self.window, textvariable=self.target_samples, width=6)
+        self.target_samples_entry = Entry(self.window, textvariable=self.target_samples, width=6,state='disabled')
         self.target_samples_entry.place(x=x_off_bottom + 450, y=y_off_bottom + 546)
 
         self.target_samples_lbl = Label(self.window, text="Enter Target frames:")
@@ -116,6 +116,9 @@ class TestRig_dashboard():
             del info_dict['performance_metrics']
         except KeyError:
             print('Performance Metrics not found.')
+        for key in self.data_keys:
+            null_frames_percent = (float(info_dict['null_frames'][key]) / float(info_dict['number_samples'])) * 100
+            info_dict['null_frames'][key] = "{} ({:.1f}%)".format(info_dict['null_frames'][key], null_frames_percent)
         info_txt = str(yaml.safe_dump(info_dict, allow_unicode=True, default_flow_style=False, indent=4))
         self.info_print = Label(self.window, text=info_txt, anchor="w", font=("Arial", 11), justify=LEFT)
         self.info_print.place(x=x_pos, y=y_pos)
@@ -135,7 +138,14 @@ class TestRig_dashboard():
         data_list = [mkeys] + data_list
         x2 = 0
         y2 = 0
+
         self.metric_title = Label(self.window, text=error_key + ' Metrics', font=("Arial", 13, 'bold'))
+
+        if error_key == 'angle_rotation':
+            self.metric_title.configure(text=error_key + ' Metrics (deg)')
+        if error_key == 'euclidean_error':
+            self.metric_title.configure(text=error_key + ' Metrics (meter)')
+
         self.metric_title.place(x=pos_x, y=pos_y - 25)
         for i in range(n_rows):
             for j in range(n_columns):
@@ -279,7 +289,7 @@ class TestRig_dashboard():
         test_rig.populate_performance_metrics()
         self.test_rig = test_rig
         self.metrics_table_print(self.x_off_mid + 130, self.y_off_mid + 420, 'euclidean_error')
-        self.metrics_table_print(self.x_off_mid + 130, self.y_off_mid + 620, 'angle_rotation_error')
+        self.metrics_table_print(self.x_off_mid + 130, self.y_off_mid + 620, 'angle_rotation')
         self.test_data_info(480, 140)
         return test_rig