main_of_IABC.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "c0a6353d",
   "metadata": {},
   "source": [
    "# Iteratively Automatic Machine Learning Boosted Hand-eye Calibration Method for a Laser Displacement Sensor\n",
    "\n",
    "* The suffix of \"_hs\" in following codes is the same with suffix \"_TS\" in the paper\n",
    "* All the codes are developed by the author except the imported libraries.\n",
    "* Any problem in usage, please feel free to contact us (Helin Li, lihelin@tju.edu.cn)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0ecaa3e9",
   "metadata": {},
   "source": [
    "## Load Library"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7d9cb94f",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-06-07T09:41:45.174695Z",
     "start_time": "2022-06-07T09:41:43.904551Z"
    }
   },
   "outputs": [],
   "source": [
    "#  jupyter_contrib_nbextensions:https://blog.csdn.net/qq_40235133/article/details/108858873\n",
    "from scipy.optimize import least_squares\n",
    "# import open3d as o3d\n",
    "import numpy as np\n",
    "import pandas as pd\n",
    "from numpy import sqrt,sin,cos\n",
    "from skspatial.objects import Line\n",
    "from skspatial.objects import Sphere\n",
    "from IPython.core.display import display, HTML\n",
    "import plotly.express as px\n",
    "from utils.display_pcd_o3d import *\n",
    "import openpyxl,pyperclip\n",
    "import math,time,datetime,pickle\n",
    "import matplotlib.pyplot as plt\n",
    "from joblib import dump, load\n",
    "display(HTML(\"<style>.container { width:80% !important; }</style>\"))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9b914801",
   "metadata": {},
   "source": [
    "## Load the training data"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0988c42a",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-06-07T09:41:47.861915Z",
     "start_time": "2022-06-07T09:41:47.563924Z"
    },
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "fname_train_slim=\"calib_zg2_acid2_1_small_sphere\"\n",
    "fname_train=\"data/Eid_zg2_ACid1_2_calibration_small_sphere_R10nY10nZ10F1P20.xyzprepro_filtered_250K_SOR.csv\"\n",
    "\n",
    "big_sphere_r=12.7075\n",
    "small_sphere_r=9.9969\n",
    "\n",
    "if( \"small\" in fname_train) or ( \"check_\" in fname_train):\n",
    "    sphere_r=small_sphere_r\n",
    "else:\n",
    "    sphere_r=big_sphere_r # the radius of the standard sphere\n",
    "train_col_names='xs_zs_alpha_beta' # the trainning feature:xs_zs_alpha_beta or pid_dis_alpha_beta, dis_alpha_beta\n",
    "y_colnames=\"xz\" # z or xz\n",
    "fit_sphere_method=\"TLS\" # LLS,TLS ，TLS has better results, but sometimse it couldnot return valid results!!, so we try the LLS\n",
    "lds_emit_pt=np.array([0,0,80]) # the virtual emit point of the LDS\n",
    "is_train_pid=True # considering mid\n",
    "is_add_compensation=1\n",
    "df=pd.read_csv(fname_train)\n",
    "df"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "02539e73",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-11T23:52:45.738374Z",
     "start_time": "2022-03-11T23:52:45.733358Z"
    }
   },
   "outputs": [],
   "source": [
    "sphere_r"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f0694678",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-11T23:52:45.742367Z",
     "start_time": "2022-03-11T23:52:45.739341Z"
    }
   },
   "outputs": [],
   "source": [
    "df.shape\n",
    "df_lsq=df"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "defb1066",
   "metadata": {},
   "source": [
    "## Down sample"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8269c16b",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-11T23:52:45.768296Z",
     "start_time": "2022-03-11T23:52:45.744330Z"
    }
   },
   "outputs": [],
   "source": [
    "'''display some data of the LDS'''\n",
    "n_sample_init=250*1000\n",
    "if df.shape[0]<=n_sample_init:\n",
    "    df_lsq=df\n",
    "else:\n",
    "    df_lsq=df.sample(n=n_sample_init,random_state=0)\n",
    "# px.scatter(df_lsq,x='x_s',y='z_s').show()\n",
    "df_lsq=df_lsq.reset_index()\n",
    "df_lsq"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "febcb00a",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-11T23:52:45.772291Z",
     "start_time": "2022-03-11T23:52:45.769261Z"
    }
   },
   "outputs": [],
   "source": [
    "# fname_save=\"F:/0实验数据/20220219-zg2/calibration/Eid_zg2_ACid1_2_calibration_small_sphere_R10nY10nZ10F1P20.xyzprepro_filtered_250K_SOR.csv\"\n",
    "# df_lsq.to_csv(fname_save)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7260c2c4",
   "metadata": {},
   "source": [
    "## Define utility functions for Training"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0cb179ff",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-11T23:52:45.811150Z",
     "start_time": "2022-03-11T23:52:45.773251Z"
    }
   },
   "outputs": [],
   "source": [
    "'''objective whose r is known'''\n",
    "def fun(X, x_bh, y_bh, z_bh, x_s, z_s,fit_sphere_method=\"TLS\",sphere_r=sphere_r):\n",
    "    '''\n",
    "    这个fun应该升级为有球心坐标的，r的已知和未知可以设定\n",
    "    define the Eyehand Formula\n",
    "    X: x_hs,y_hs,z_hs,alpha_hs,beta_hs,gamma_hs: equivalent eye-hand parameters, to be identified\n",
    "    x_bh,y_bh,z_bh: displacement of X,Y,Z of the machine\n",
    "    x_s,z_s: indications of the LDS\n",
    "    '''\n",
    "    x_hs, y_hs, z_hs, alpha_hs, beta_hs, gamma_hs = X\n",
    "    r = sphere_r\n",
    "    if fit_sphere_method==\"TLS\":\n",
    "        return -r + sqrt((x_bh + x_s * cos(beta_hs) * cos(gamma_hs) + x_hs + z_s * sin(beta_hs)) ** 2 + (\n",
    "                x_s * sin(alpha_hs) * sin(gamma_hs) - x_s * sin(beta_hs) * cos(alpha_hs) * cos(\n",
    "            gamma_hs) + z_bh + z_hs + z_s * cos(alpha_hs) * cos(beta_hs)) ** 2 + (\n",
    "                                 x_s * sin(alpha_hs) * sin(beta_hs) * cos(gamma_hs) + x_s * sin(gamma_hs) * cos(\n",
    "                             alpha_hs) + y_bh + y_hs - z_s * sin(alpha_hs) * cos(beta_hs)) ** 2)\n",
    "    else:\n",
    "        return -r**2 + ((x_bh + x_s * cos(beta_hs) * cos(gamma_hs) + x_hs + z_s * sin(beta_hs)) ** 2 + (\n",
    "                x_s * sin(alpha_hs) * sin(gamma_hs) - x_s * sin(beta_hs) * cos(alpha_hs) * cos(\n",
    "            gamma_hs) + z_bh + z_hs + z_s * cos(alpha_hs) * cos(beta_hs)) ** 2 + (\n",
    "                                 x_s * sin(alpha_hs) * sin(beta_hs) * cos(gamma_hs) + x_s * sin(gamma_hs) * cos(\n",
    "                             alpha_hs) + y_bh + y_hs - z_s * sin(alpha_hs) * cos(beta_hs)) ** 2)\n",
    "\n",
    "'''objective whose r is unknown'''\n",
    "def fun_r(X, x_bh, y_bh, z_bh, x_s, z_s,fit_sphere_method=\"TLS\",sphere_r=sphere_r):\n",
    "    '''\n",
    "    这个fun应该升级为有球心坐标的，r的已知和未知可以设定\n",
    "    define the Eyehand Formula\n",
    "    X: x_hs,y_hs,z_hs,alpha_hs,beta_hs,gamma_hs: equivalent eye-hand parameters, to be identified\n",
    "    x_bh,y_bh,z_bh: displacement of X,Y,Z of the machine\n",
    "    x_s,z_s: indications of the LDS\n",
    "    '''\n",
    "    x_hs, y_hs, z_hs, alpha_hs, beta_hs, gamma_hs, r = X\n",
    "    if fit_sphere_method==\"TLS\":\n",
    "        return -r + sqrt((x_bh + x_s * cos(beta_hs) * cos(gamma_hs) + x_hs + z_s * sin(beta_hs)) ** 2 + (\n",
    "                x_s * sin(alpha_hs) * sin(gamma_hs) - x_s * sin(beta_hs) * cos(alpha_hs) * cos(\n",
    "            gamma_hs) + z_bh + z_hs + z_s * cos(alpha_hs) * cos(beta_hs)) ** 2 + (\n",
    "                                 x_s * sin(alpha_hs) * sin(beta_hs) * cos(gamma_hs) + x_s * sin(gamma_hs) * cos(\n",
    "                             alpha_hs) + y_bh + y_hs - z_s * sin(alpha_hs) * cos(beta_hs)) ** 2)\n",
    "    else:\n",
    "        return -r**2 + ((x_bh + x_s * cos(beta_hs) * cos(gamma_hs) + x_hs + z_s * sin(beta_hs)) ** 2 + (\n",
    "                x_s * sin(alpha_hs) * sin(gamma_hs) - x_s * sin(beta_hs) * cos(alpha_hs) * cos(\n",
    "            gamma_hs) + z_bh + z_hs + z_s * cos(alpha_hs) * cos(beta_hs)) ** 2 + (\n",
    "                                 x_s * sin(alpha_hs) * sin(beta_hs) * cos(gamma_hs) + x_s * sin(gamma_hs) * cos(\n",
    "                             alpha_hs) + y_bh + y_hs - z_s * sin(alpha_hs) * cos(beta_hs)) ** 2)\n",
    "\n",
    "'''(this is the high Speed version!)Vectorially Calculate the intersection point of sphere and lines'''\n",
    "def intersectSphereAndLineVec(sphere_center_pt, sphere_r, lds_emit_pt, lds_meas_pt):\n",
    "    v_lc = sphere_center_pt - lds_emit_pt\n",
    "    v_lp = lds_meas_pt - lds_emit_pt\n",
    "    v_lp_norm = np.linalg.norm(x=v_lp, ord=2, axis=1)\n",
    "#     print(f\"sphere_center_pt_in_LDS_CS:{sphere_center_pt}\")\n",
    "#     print(f\"lds_emit_pt:{lds_emit_pt}\")\n",
    "#     print(f\"v_lc:{v_lc}\")\n",
    "#     print(f'v_lp_norm:{v_lp_norm}')\n",
    "    v_lp_unit = v_lp / v_lp_norm[:, None]  # X[:,None] Broadcast the 1D  Ndarray to 2D  Ndarray\n",
    "    ld = np.sum(v_lp_unit * v_lc, axis=1)\n",
    "    v_lc_norm = np.linalg.norm(x=v_lc, ord=2,axis=1)\n",
    "#     print(f'v_lc_norm:{v_lc_norm}')\n",
    "    r_square = sphere_r ** 2 - ((v_lc_norm) ** 2 - ld ** 2)\n",
    "#     pd.DataFrame(r_square).to_csv('r_square.csv')\n",
    "    r = np.sqrt(r_square)\n",
    "    q1 = lds_emit_pt + v_lp_unit * (ld - r)[:, None]  # the nearest intersection point\n",
    "    q2 = lds_emit_pt+v_lp_unit*(ld+r)[:,None]\n",
    "#     pd.DataFrame(q1).to_csv(\"q1.csv\")\n",
    "#     pd.DataFrame(q2).to_csv(\"q2.csv\")\n",
    "    return q1,q2\n",
    "\n",
    "'''Calculate the Point cloud by the optimal eye-hand matrix'''\n",
    "def calPcdByEyehand(params_eyehand_est,df):\n",
    "    if isinstance(params_eyehand_est, pd.DataFrame) or  isinstance(params_eyehand_est, pd.Series):\n",
    "        d=params_eyehand_est\n",
    "        x_hs, y_hs, z_hs, alpha_hs, beta_hs, gamma_hs = d.x_hs, d.y_hs, d.z_hs, d.alpha_hs, d.beta_hs, d.gamma_hs\n",
    "        print(f\"params_eyehand_est:{params_eyehand_est}\")\n",
    "        print(\"x_hs, y_hs, z_hs, alpha_hs, beta_hs, gamma_hs\",x_hs, y_hs, z_hs, alpha_hs, beta_hs, gamma_hs)\n",
    "#     print(f\"type of df:{type(df)}\")\n",
    "    else:\n",
    "        x_hs, y_hs, z_hs, alpha_hs, beta_hs, gamma_hs = params_eyehand_est\n",
    "    if isinstance(df, tuple):\n",
    "        x_bh,y_bh,z_bh,x_s,z_s=df\n",
    "    else: # args_train tulple\n",
    "        x_bh,y_bh,z_bh,x_s,z_s=df.loc[:,\"x_bh\"],df.loc[:,\"y_bh\"],df.loc[:,\"z_bh\"],df.loc[:,\"x_s\"],df.loc[:,\"z_s\"]\n",
    "    return np.array([\n",
    "        x_bh + x_hs + x_s*cos(beta_hs)*cos(gamma_hs) + z_s*sin(beta_hs), \n",
    "        x_s*sin(alpha_hs)*sin(beta_hs)*cos(gamma_hs) + x_s*sin(gamma_hs)*cos(alpha_hs) + y_bh + y_hs - z_s*sin(alpha_hs)*cos(beta_hs), \n",
    "        x_s*sin(alpha_hs)*sin(gamma_hs) - x_s*sin(beta_hs)*cos(alpha_hs)*cos(gamma_hs) + z_bh + z_hs + z_s*cos(alpha_hs)*cos(beta_hs), \n",
    "    ]).T\n",
    "\n",
    "'''Define the objective of fitting the sphere with known r'''\n",
    "def objectiveFitSphere(parameters, x_values, y_values, z_values,r=sphere_r):\n",
    "    # extract the parameters\n",
    "    x_centre, y_centre, z_centre = parameters\n",
    "\n",
    "#     print(\"x_centre, y_centre, z_centre\")\n",
    "#     print(x_centre, y_centre, z_centre)\n",
    "#     print(\" x_values, y_values, z_values,r\")\n",
    "#     print(x_values.shape, y_values.shape, z_values.shape,r)\n",
    "    # use np's sqrt function here, which works by element on arrays\n",
    "    distance_from_centre = np.sqrt((x_values - x_centre) ** 2 +\n",
    "                                      (y_values - y_centre) ** 2 +\n",
    "                                      (z_values - z_centre) ** 2)-r\n",
    "    return distance_from_centre\n",
    "\n",
    "'''Define the objective of fitting the sphere with unkown r'''\n",
    "def objective2FitSphere(parameters, x_values, y_values, z_values,fit_sphere_method):\n",
    "    # extract the parameters\n",
    "    x_centre, y_centre, z_centre,r = parameters\n",
    "    # use np's sqrt function here, which works by element on arrays\n",
    "    if fit_sphere_method ==\"TLS\":\n",
    "        distance_from_centre = np.sqrt((x_values - x_centre) ** 2 +(y_values - y_centre) ** 2 +(z_values - z_centre) ** 2)-r\n",
    "    else: # LLS\n",
    "        distance_from_centre =((x_values - x_centre) ** 2 +(y_values - y_centre) ** 2 +(z_values - z_centre) ** 2)-r**2\n",
    "    return distance_from_centre\n",
    "\n",
    "'''Fit sphere with known r'''\n",
    "def fitSphere(df,r=sphere_r,n=5):\n",
    "    '''\n",
    "    parameter df: pandas DataFrame, or numpy Ndarray\n",
    "    '''\n",
    "    res_fs_arr=[]\n",
    "#     print(f\"df:{df}\")\n",
    "    if isinstance(df, pd.DataFrame):\n",
    "        args=(df.x,df.y,df.z,r)\n",
    "    else:\n",
    "        args=(df[:,0],df[:,1],df[:,2],r)\n",
    "    for i in range(n):\n",
    "        x0_fit_sphere= np.random.random(3)\n",
    "#         print(f\"x0_fit_sphere:{x0_fit_sphere}\")\n",
    "#         print(f\"args:{args}\")\n",
    "        fs_res=least_squares(fun=objectiveFitSphere,x0=x0_fit_sphere,args=args,method='trf',loss='huber')\n",
    "        rmse=np.sqrt(np.mean(fs_res.fun**2))\n",
    "        mae=np.mean(np.abs(fs_res.fun))\n",
    "        res_i=[*fs_res.x,rmse,mae]\n",
    "#         print(f\"fitSphere_res_i:{res_i}\")\n",
    "        res_fs_arr.append(res_i)\n",
    "    return pd.DataFrame(res_fs_arr,columns=['x_c','y_c','y_c','rmse','mae'])\n",
    "\n",
    "'''Fit R with unknwon r'''\n",
    "def fitSphereR(df,n=5,r=None,fit_sphere_method=\"TLS\"):\n",
    "    res_fs_arr=[]\n",
    "    for i in range(n):\n",
    "        if isinstance(df, pd.DataFrame):\n",
    "            args=(df.x,df.y,df.z,fit_sphere_method)\n",
    "        else:\n",
    "            args=(df[:,0],df[:,1],df[:,2],fit_sphere_method)\n",
    "        \n",
    "        x0_fit_sphere= np.random.random(4)\n",
    "        x0_fit_sphere[3]=r\n",
    "        bounds=([-np.inf,-np.inf,-np.inf,0.8*r],[np.inf,np.inf,np.inf,1.2*r])\n",
    "        fs_res=least_squares(fun=objective2FitSphere,x0=x0_fit_sphere,args=args,method='trf',loss='huber',bounds=bounds)\n",
    "#         fs_res=least_squares(fun=objective2FitSphere,x0=x0_fit_sphere,args=args,method='lm',loss='linear')\n",
    "        rmse=np.sqrt(np.mean(fs_res.fun**2))\n",
    "        mae=np.mean(np.abs(fs_res.fun))\n",
    "#         print(fs_res)\n",
    "        err_r=r-fs_res.x[3]\n",
    "        err_r_rate=100*err_r/r\n",
    "        res_i=[*fs_res.x,rmse,err_r,mae,err_r_rate]\n",
    "#         print(f\"fitSphere_res_i:{res_i}\")\n",
    "        res_fs_arr.append(res_i)\n",
    "    return pd.DataFrame(res_fs_arr,columns=['x_c','y_c','y_c','r','rmse','err_r','mae','err_r(%)'])\n",
    "\n",
    "def sphereFitByLsq2(point_cloud):\n",
    "    '''\n",
    "    ref:https://jekel.me/2015/Least-Squares-Sphere-Fit/ (test_time:10ms)\n",
    "    '''\n",
    "    #   Assemble the A matrix\n",
    "    spX = point_cloud[:,0]\n",
    "    spY = point_cloud[:,1]\n",
    "    spZ = point_cloud[:,2]\n",
    "    A = np.zeros((len(spX),4))\n",
    "    A[:,0] = spX*2\n",
    "    A[:,1] = spY*2\n",
    "    A[:,2] = spZ*2\n",
    "    A[:,3] = 1\n",
    "\n",
    "    #   Assemble the f matrix\n",
    "    f = np.zeros((len(spX),1))\n",
    "    f[:,0] = (spX*spX) + (spY*spY) + (spZ*spZ)\n",
    "    C, residules, rank, singval = np.linalg.lstsq(A,f)\n",
    "\n",
    "    #   solve for the radius\n",
    "    t = (C[0]*C[0])+(C[1]*C[1])+(C[2]*C[2])+C[3]\n",
    "    radius = math.sqrt(t)\n",
    "\n",
    "    return radius, C[0], C[1], C[2]\n",
    "\n",
    "'''SOR: Remove statistical outlier 3d'''\n",
    "def removeStatisticalOutlier3d(pcd_o3d,nb_neighbors=30,std_ratio=3.0,is_show=False):\n",
    "    cl, ind = pcd_o3d.remove_statistical_outlier(nb_neighbors=nb_neighbors,std_ratio=std_ratio)\n",
    "    pcd_inlier = pcd_o3d.select_by_index(ind)\n",
    "    displayInlierOutlier(pcd_o3d, ind) if is_show else _\n",
    "    print(f\"Inlier Data:{len(ind)}(Inlier ratio: {len(ind)*100/(np.asarray(pcd_o3d.points)).shape[0]}%)\")\n",
    "    return pcd_inlier,ind\n",
    "\n",
    "'''calculate the signed angle from 2d vector v1 to v2'''\n",
    "def cal2DSignedAngleFromV1ToV2Vec(v1,v2,units='rad',is_signed=True):\n",
    "    dot_vec=np.sum(np.multiply(v1,v2), axis=1) # 1D array's dot equlas this operation, for high speed vector caculation\n",
    "    if not is_signed: # principle: cos(<a,b>)=a*b/(|a|*{b})\n",
    "        res_arctan2 = np.abs\n",
    "        norm_vec=(np.linalg.norm(v1,axis=1))*(np.linalg.norm(v2,axis=1))\n",
    "        res_arccos=np.arccos(dot_vec/norm_vec)\n",
    "        return res_arccos if units=='rad' else np.rad2deg(res_arccos)\n",
    "    else: # principle: sin(<a,b>)=a×b/(|a|*{b}),cos(<a,b>)=a*b/(|a|*{b}),so,<a,b>=arctan(sin/cos)\n",
    "        a11, a12, a21, a22 = v1[:, 0], v1[:, 1], v2[:, 0], v2[:, 1]\n",
    "        det_vec= a11*a22-a12*a21 # second order determinant calculation formula\n",
    "        res_arctan2= np.arctan2(det_vec,dot_vec)\n",
    "#         print(f\"res_arctan2:{res_arctan2}\")\n",
    "#         print(f\"res_arctan2:{np.rad2deg(res_arctan2)}\")\n",
    "        return res_arctan2 if units=='rad' else np.rad2deg(res_arctan2)\n",
    "\n",
    "'''Calculate the shpere center in LDS_CS'''\n",
    "'''TODO : ???right??? '''\n",
    "def calSphereCenterInSensorCS(params_eyehand_est,xyz_bh):\n",
    "    if isinstance(params_eyehand_est, pd.DataFrame) or  isinstance(params_eyehand_est, pd.Series):\n",
    "        d=params_eyehand_est\n",
    "        x_hs, y_hs, z_hs, alpha_hs, beta_hs, gamma_hs = d.x_hs, d.y_hs, d.z_hs, d.alpha_hs, d.beta_hs, d.gamma_hs\n",
    "        print(f\"params_eyehand_est:{params_eyehand_est}\")\n",
    "        print(\"x_hs, y_hs, z_hs, alpha_hs, beta_hs, gamma_hs\",x_hs, y_hs, z_hs, alpha_hs, beta_hs, gamma_hs)\n",
    "#     print(f\"type of df:{type(df)}\")\n",
    "    else:\n",
    "        x_hs, y_hs, z_hs, alpha_hs, beta_hs, gamma_hs = params_eyehand_est\n",
    "        \n",
    "#     x_hs, y_hs, z_hs, alpha_hs, beta_hs, gamma_hs = params_eyehand_est\n",
    "    x_bh,y_bh,z_bh=xyz_bh\n",
    "    return np.asarray([\n",
    "        -x_bh*cos(beta_hs)*cos(gamma_hs) - x_hs*cos(beta_hs)*cos(gamma_hs) - y_bh*sin(alpha_hs)*sin(beta_hs)*cos(gamma_hs) - y_bh*sin(gamma_hs)*cos(alpha_hs) - y_hs*sin(alpha_hs)*sin(beta_hs)*cos(gamma_hs) - y_hs*sin(gamma_hs)*cos(alpha_hs) - z_bh*sin(alpha_hs)*sin(gamma_hs) + z_bh*sin(beta_hs)*cos(alpha_hs)*cos(gamma_hs) - z_hs*sin(alpha_hs)*sin(gamma_hs) + z_hs*sin(beta_hs)*cos(alpha_hs)*cos(gamma_hs),\n",
    "        x_bh*sin(gamma_hs)*cos(beta_hs) + x_hs*sin(gamma_hs)*cos(beta_hs) + y_bh*sin(alpha_hs)*sin(beta_hs)*sin(gamma_hs) - y_bh*cos(alpha_hs)*cos(gamma_hs) + y_hs*sin(alpha_hs)*sin(beta_hs)*sin(gamma_hs) - y_hs*cos(alpha_hs)*cos(gamma_hs) - z_bh*sin(alpha_hs)*cos(gamma_hs) - z_bh*sin(beta_hs)*sin(gamma_hs)*cos(alpha_hs) - z_hs*sin(alpha_hs)*cos(gamma_hs) - z_hs*sin(beta_hs)*sin(gamma_hs)*cos(alpha_hs),\n",
    "        -x_bh*sin(beta_hs) - x_hs*sin(beta_hs) + y_bh*sin(alpha_hs)*cos(beta_hs) + y_hs*sin(alpha_hs)*cos(beta_hs) - z_bh*cos(alpha_hs)*cos(beta_hs) - z_hs*cos(alpha_hs)*cos(beta_hs),\n",
    "        ]).T\n",
    "\n",
    "'''Estimate the distance, alpha and beta of the LDS in LDS_Frame for any scattered point cloud'''\n",
    "def estimateDisAndAlphaAndBetaInLdsFrame(df,params_eyehand_est):\n",
    "    '''Calculate the lds_dis_est'''\n",
    "    vec_PL= lds_emit_pt-df.loc[:,\"x_s\":'z_s'].to_numpy() # the vector from the virtual emitting point to the measured point\n",
    "    lds_dis_est=np.linalg.norm(vec_PL,axis=1)\n",
    "    \n",
    "    '''Estimate the signed lds_alpha_est'''\n",
    "    pts_normal_end=calNormalPointsInLdsFrame(df=df,params_eyehand_est=params_eyehand_est,is_start_point=False)\n",
    "    pts_normal_start=df.loc[:,'x_s':'z_s'].to_numpy() # the measured point, also the start point of normals\n",
    "    vec_CP= pts_normal_end-pts_normal_start # the nomal vector    \n",
    "    normal_lds=vec_CP\n",
    "    \n",
    "#     print(f\"vec_CP:{vec_CP.shape}\")\n",
    "    vec_CP_in_XZ=vec_CP[:,[0,2]] # get the XZ plane's projection of vec_CP\n",
    "    vec_PL_in_XZ=vec_PL[:,[0,2]] #　the vector from measured point to the lds's virtual emitting point\n",
    "    lds_alpha_est = cal2DSignedAngleFromV1ToV2Vec(v1=vec_PL_in_XZ,v2=vec_CP_in_XZ)\n",
    "    # TODO: lds_alpha_est must not greater than 90°    \n",
    "    \n",
    "    '''Estimate the signed lds_beta_est'''\n",
    "    # TODO: lds_beta_est must not greater than 90°角度正负问题，角度范围自动处理问题，\n",
    "#     vec_CP_in_XZ_norm=np.linalg.norm(x=vec_CP_in_XZ,axis=1)\n",
    "#     lds_beta_est= np.arctan2(vec_CP[:,1],vec_CP_in_XZ_norm) # arctan2(y-coordinates,x-coordinates)\n",
    "    lds_beta_est= np.arctan2(-vec_CP[:,1],vec_CP[:,2]) # arctan2(y-coordinates,x-coordinates)\n",
    "    \n",
    "    # normal_dis_alpha_beta_lds\n",
    "    return np.column_stack([normal_lds,lds_dis_est,lds_alpha_est,lds_beta_est])\n",
    "\n",
    "\n",
    "'''Calculate the normal's start point or end point in LDS Frame '''\n",
    "def calNormalPointsInLdsFrame(df,params_eyehand_est,is_start_point=False):\n",
    "    x_hs, y_hs, z_hs, alpha_hs, beta_hs, gamma_hs = params_eyehand_est\n",
    "    x_bh,y_bh,z_bh,x_s,z_s=df.loc[:,\"x_bh\"],df.loc[:,\"y_bh\"],df.loc[:,\"z_bh\"],df.loc[:,\"x_s\"],df.loc[:,\"z_s\"]\n",
    "    if is_start_point:\n",
    "        x_wp,y_wp,z_wp=df.loc[:,\"x\"],df.loc[:,\"y\"],df.loc[:,\"z\"]\n",
    "    else:\n",
    "        x_wp,y_wp,z_wp=df.loc[:,\"x\"]+df.loc[:,\"nx\"],df.loc[:,\"y\"]+df.loc[:,\"ny\"],df.loc[:,\"z\"]+df.loc[:,\"nz\"]\n",
    "    pts=np.asarray([\n",
    "        -x_bh*cos(beta_hs)*cos(gamma_hs) - x_hs*cos(beta_hs)*cos(gamma_hs)\n",
    "         + x_wp*cos(beta_hs)*cos(gamma_hs) - y_bh*sin(alpha_hs)*sin(beta_hs)*cos(gamma_hs)\n",
    "         - y_bh*sin(gamma_hs)*cos(alpha_hs) - y_hs*sin(alpha_hs)*sin(beta_hs)*cos(gamma_hs)\n",
    "         - y_hs*sin(gamma_hs)*cos(alpha_hs) + y_wp*sin(alpha_hs)*sin(beta_hs)*cos(gamma_hs)\n",
    "         + y_wp*sin(gamma_hs)*cos(alpha_hs) - z_bh*sin(alpha_hs)*sin(gamma_hs)\n",
    "         + z_bh*sin(beta_hs)*cos(alpha_hs)*cos(gamma_hs) - z_hs*sin(alpha_hs)*sin(gamma_hs)\n",
    "         + z_hs*sin(beta_hs)*cos(alpha_hs)*cos(gamma_hs) + z_wp*sin(alpha_hs)*sin(gamma_hs)\n",
    "         - z_wp*sin(beta_hs)*cos(alpha_hs)*cos(gamma_hs), \n",
    "        x_bh*sin(gamma_hs)*cos(beta_hs) + x_hs*sin(gamma_hs)*cos(beta_hs) \n",
    "         - x_wp*sin(gamma_hs)*cos(beta_hs) + y_bh*sin(alpha_hs)*sin(beta_hs)*sin(gamma_hs)\n",
    "         - y_bh*cos(alpha_hs)*cos(gamma_hs) + y_hs*sin(alpha_hs)*sin(beta_hs)*sin(gamma_hs)\n",
    "         - y_hs*cos(alpha_hs)*cos(gamma_hs) - y_wp*sin(alpha_hs)*sin(beta_hs)*sin(gamma_hs)\n",
    "         + y_wp*cos(alpha_hs)*cos(gamma_hs) - z_bh*sin(alpha_hs)*cos(gamma_hs) \n",
    "         - z_bh*sin(beta_hs)*sin(gamma_hs)*cos(alpha_hs) - z_hs*sin(alpha_hs)*cos(gamma_hs)\n",
    "         - z_hs*sin(beta_hs)*sin(gamma_hs)*cos(alpha_hs) + z_wp*sin(alpha_hs)*cos(gamma_hs)\n",
    "         + z_wp*sin(beta_hs)*sin(gamma_hs)*cos(alpha_hs), \n",
    "        -x_bh*sin(beta_hs) - x_hs*sin(beta_hs) + x_wp*sin(beta_hs)\n",
    "         + y_bh*sin(alpha_hs)*cos(beta_hs) + y_hs*sin(alpha_hs)*cos(beta_hs) \n",
    "         - y_wp*sin(alpha_hs)*cos(beta_hs) - z_bh*cos(alpha_hs)*cos(beta_hs) \n",
    "         - z_hs*cos(alpha_hs)*cos(beta_hs) + z_wp*cos(alpha_hs)*cos(beta_hs),\n",
    "        ]).T\n",
    "    return pts"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "fc27c808",
   "metadata": {},
   "source": [
    "## Preliminary estimate the eye-hand parameters ( also the traditional calibration methods)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c339fa05",
   "metadata": {},
   "source": [
    "### Estimate initial eye-hand parameters with know r's objective (also present the traditional calibration methods)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c80922f2",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T02:13:58.338061Z",
     "start_time": "2022-03-12T02:13:51.771514Z"
    },
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "%%time\n",
    "'''Estimate eye-hand'''\n",
    "lsq_res_arr=[]\n",
    "lsq_x0_arr=[]\n",
    "j=0\n",
    "\n",
    "fit_sphere_method='LLS'\n",
    "loss='linear'\n",
    "method_solving='lm'\n",
    "verbose=0\n",
    "\n",
    "if fit_sphere_method==\"LLS\":\n",
    "    rmse_threhold=0.7\n",
    "else:\n",
    "    rmse_threhold=0.05\n",
    "print(f\"Target_R:{sphere_r},Residual:{fit_sphere_method},Loss:{loss},Solving:{method_solving}\")\n",
    "\n",
    "while True:\n",
    "    j=j+1\n",
    "    if j<10:\n",
    "        is_valid=False\n",
    "        for i in range(3):\n",
    "            args_train=(df_lsq.x_bh,df_lsq.y_bh,df_lsq.z_bh,df_lsq.x_s,df_lsq.z_s,fit_sphere_method,sphere_r)\n",
    "            x0 = np.random.random(6)\n",
    "            lsq_x0_arr.append(x0)\n",
    "            bounds = ([-np.inf, -np.inf, -np.inf, -np.pi, -np.pi, -np.pi], [np.inf, np.inf, np.inf, np.pi, np.pi, np.pi])\n",
    "            if method_solving == \"trf\":\n",
    "                res_lsq0 = least_squares(fun=fun, x0=x0, args=args_train, method=method_solving, loss=loss,bounds=bounds,verbose=verbose)\n",
    "            else:  # 'lm'\n",
    "                res_lsq0 = least_squares(fun=fun, x0=x0, args=args_train, method=method_solving, loss=loss,verbose=verbose)\n",
    "            rmse=np.sqrt(np.mean(res_lsq0.fun**2))\n",
    "            mae=np.mean(np.abs(res_lsq0.fun))\n",
    "            print(rmse,mae)\n",
    "            lsq_res_arr.append([*res_lsq0.x,rmse,mae])\n",
    "            if rmse<rmse_threhold:\n",
    "                is_valid=True\n",
    "    else:\n",
    "        print('Error! Please check the Data!')\n",
    "    if is_valid:\n",
    "        break\n",
    "df_lsq_res=pd.DataFrame(lsq_res_arr,columns=[ 'x_hs', 'y_hs', 'z_hs', 'alpha_hs', 'beta_hs', 'gamma_hs','rmse','mae'])\n",
    "df_lsq_res.loc[:,'alpha_deg']=np.rad2deg(df_lsq_res.alpha_hs)\n",
    "df_lsq_res.loc[:,'beta_deg']=np.rad2deg(df_lsq_res.beta_hs)\n",
    "df_lsq_res.loc[:,'gamma_deg']=np.rad2deg(df_lsq_res.gamma_hs)\n",
    "df_lsq_res"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "750d0d5a",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T02:13:58.558500Z",
     "start_time": "2022-03-12T02:13:58.544508Z"
    }
   },
   "outputs": [],
   "source": [
    "df_lsq_res=df_lsq_res.sort_values(by=\"rmse\").reset_index(drop=True)\n",
    "df_lsq_res"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6dfa33ed",
   "metadata": {},
   "source": [
    "### Fit a sphere by aboving eye-hand parameters\n",
    "fit a shpere before training our model for comparision,the result represent traditional eye-hand calibration methods"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "49c88907",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T02:14:01.745869Z",
     "start_time": "2022-03-12T02:13:58.764919Z"
    }
   },
   "outputs": [],
   "source": [
    "fit_sphere_method='TLS'\n",
    "params_eyehand_est_lsq_=df_lsq_res.sort_values(by=\"rmse\").iloc[0]\n",
    "pcd1_ndarr=calPcdByEyehand(params_eyehand_est=params_eyehand_est_lsq_,df=df_lsq)\n",
    "df_pcd1=pd.DataFrame(pcd1_ndarr,columns=['x','y','z'])\n",
    "df_pcd1.to_csv(f\"res/pcd/{fname_train_slim}_pcd_init.csv\",index=False)\n",
    "df_fit_sph_res1=fitSphereR(df_pcd1,r=sphere_r,n=3,fit_sphere_method=fit_sphere_method)\n",
    "df_fit_sph_res1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "205ea4c1",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T02:15:22.920742Z",
     "start_time": "2022-03-12T02:15:22.914759Z"
    }
   },
   "outputs": [],
   "source": [
    "print(params_eyehand_est_lsq_.to_numpy()[:6])\n",
    "# x0_lls_linear_lm=np.asarray([-80.34245759, -15.54366193, -61.60630461,   1.53055302  , 0.0866374,   1.5362051 ])\n",
    "# x0_lls_linear_lm_unknownR=np.asarray([-80.3425403 , -15.57389033 ,-61.60505205 ,  4.67214613  , 3.05498474,4.67778049 ])\n",
    "# x0_tls_linear_lm_unknownR=np.asarray([-8.03425436e+01, -1.55744357e+01, -6.16050345e+01 ,-4.75263057e+00,  8.66082105e-02, -4.74699597e+00 ])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "70ea8617",
   "metadata": {},
   "source": [
    "### Estimate initial eye-hand parameters with unknow r's objective"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "15eb35ea",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T02:10:14.426249Z",
     "start_time": "2022-03-12T02:10:05.160331Z"
    },
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "'''Estimate eye-hand and r'''\n",
    "fit_sphere_method=\"LLS\" \n",
    "loss='linear'\n",
    "method_solving='lm'\n",
    "verbose=0\n",
    "print(f\"Target_R:{sphere_r},Residual:{fit_sphere_method},Loss:{loss},Solving:{method_solving}\")\n",
    "lsq_res_r_arr=[]\n",
    "lsq_x0_r_arr=[]\n",
    "for i in range(3):\n",
    "    args_train=(df_lsq.x_bh,df_lsq.y_bh,df_lsq.z_bh,df_lsq.x_s,df_lsq.z_s,fit_sphere_method,sphere_r)\n",
    "    x0_r = np.random.random(7)\n",
    "    x0_r[6]=sphere_r\n",
    "    lsq_x0_r_arr.append(x0_r)\n",
    "#     bounds=([-np.inf,-np.inf,-np.inf,-np.pi,-np.pi,-np.pi,0.8*sphere_r],[np.inf,np.inf,np.inf,np.pi,np.pi,np.pi,1.2*sphere_r])\n",
    "#     bounds=([-np.inf,-np.inf,-np.inf,-np.inf,-np.inf,-np.inf,sphere_r-1],[np.inf,np.inf,np.inf,np.inf,np.inf,np.inf,sphere_r+1])\n",
    "#     res_lsq0_r = least_squares(fun=fun_r, x0=x0_r, args=args_train,method='trf',loss='huber',bounds=bounds)\n",
    "#     res_lsq0_r = least_squares(fun=fun_r, x0=x0_r, args=args_train,method='trf',loss='linear',bounds=bounds)\n",
    "#     res_lsq0_r = least_squares(fun=fun_r, x0=x0_r, args=args_train,method='lm',loss='linear')\n",
    "\n",
    "    bounds = ([-np.inf, -np.inf, -np.inf, -np.pi, -np.pi, -np.pi, 0.8 * sphere_r],\n",
    "              [np.inf, np.inf, np.inf, np.pi, np.pi, np.pi, 1.2 * sphere_r])\n",
    "    if method_solving == \"trf\":\n",
    "        res_lsq0_r = least_squares(fun=fun_r, x0=x0_r, args=args_train, method=method_solving, loss=loss,bounds=bounds,verbose=verbose)\n",
    "    else:  # 'lm'\n",
    "        res_lsq0_r = least_squares(fun=fun_r, x0=x0_r, args=args_train, method=method_solving, loss=loss,verbose=verbose)\n",
    "                    \n",
    "    rmse=np.sqrt(np.mean(res_lsq0_r.fun**2))\n",
    "    mae=np.mean(np.abs(res_lsq0_r.fun))\n",
    "    print(f\"rmse:{rmse},mae:{mae}\")\n",
    "    lsq_res_r_arr.append([*res_lsq0_r.x,rmse,mae])\n",
    "df_lsq_res_r=pd.DataFrame(lsq_res_r_arr,columns=[ 'x_hs', 'y_hs', 'z_hs', 'alpha_hs', 'beta_hs', 'gamma_hs','r','rmse','mae']).sort_values(\"rmse\")\n",
    "df_lsq_res_r"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3fe93f9d",
   "metadata": {},
   "source": [
    "### Fit a sphere by aboving eye-hand parameters"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f70d5f99",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T02:10:17.152453Z",
     "start_time": "2022-03-12T02:10:14.626721Z"
    }
   },
   "outputs": [],
   "source": [
    "df_lsq_res_r=df_lsq_res_r.sort_values(by=\"rmse\").reset_index(drop=True)\n",
    "params_eyehand_est_lsq_=df_lsq_res_r.sort_values(by=\"rmse\").iloc[0]\n",
    "pcd1_ndarr=calPcdByEyehand(params_eyehand_est=params_eyehand_est_lsq_,df=df_lsq)\n",
    "df_pcd1=pd.DataFrame(pcd1_ndarr,columns=['x','y','z'])\n",
    "df_fit_sph_res1=fitSphereR(df_pcd1,r=sphere_r)\n",
    "df_fit_sph_res1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "83bf6890",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T02:10:56.017948Z",
     "start_time": "2022-03-12T02:10:56.011993Z"
    }
   },
   "outputs": [],
   "source": [
    "print(params_eyehand_est_lsq_.to_numpy())\n",
    "x0_lls_linear_lm_unknownR=np.asarray([-80.3425403 , -15.57389033 ,-61.60505205 ,  4.67214613  , 3.05498474,4.67778049 ])\n",
    "x0_tls_linear_lm_unknownR=np.asarray([-8.03425436e+01, -1.55744357e+01, -6.16050345e+01 ,-4.75263057e+00,  8.66082105e-02, -4.74699597e+00 ])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "909f76f0",
   "metadata": {},
   "source": [
    "### Select a initial eye-hand parameter"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a1013cc7",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-02-28T10:22:17.859915Z",
     "start_time": "2022-02-28T10:22:17.856907Z"
    }
   },
   "outputs": [],
   "source": [
    "# df_lsq_res=df_lsq_res_r\n",
    "# df_lsq_res"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "493bf266",
   "metadata": {},
   "source": [
    "### Display the pcd"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2b8d7662",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-02-28T10:22:19.715714Z",
     "start_time": "2022-02-28T10:22:17.861894Z"
    }
   },
   "outputs": [],
   "source": [
    "pcd_train_o3d = o3d.geometry.PointCloud()\n",
    "pcd_train_o3d.points = o3d.utility.Vector3dVector(pcd1_ndarr)\n",
    "displayO3dIns(pcd_train_o3d)\n",
    "# np.save(pcd1_ndarr)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "93849407",
   "metadata": {},
   "source": [
    "## SOR"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b4667396",
   "metadata": {},
   "source": [
    "### SOR"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "17930e7e",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-11T23:56:59.084580Z",
     "start_time": "2022-03-11T23:56:59.080591Z"
    }
   },
   "outputs": [],
   "source": [
    "df_inlier=df_lsq"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "acc8dfed",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-10T00:44:51.083369Z",
     "start_time": "2022-03-10T00:44:51.078379Z"
    }
   },
   "outputs": [],
   "source": [
    "'''Remove statistical outlier 3D before iteration'''\n",
    "# df_lsq=df\n",
    "# pcd_train=calPcdByEyehand(params_eyehand_est=params_eyehand_est_lsq_,df=df_lsq)\n",
    "# pcd_train_o3d = o3d.geometry.PointCloud()\n",
    "# pcd_train_o3d.points = o3d.utility.Vector3dVector(pcd_train)\n",
    "# _,ind=removeStatisticalOutlier3d(pcd_train_o3d,nb_neighbors=30,std_ratio=5,is_show=1)\n",
    "# df_inlier=df_lsq.loc[ind,:]\n",
    "# df_inlier"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "23ca111c",
   "metadata": {},
   "source": [
    "### Fit a sphere"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "545309ab",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-10T00:44:54.163128Z",
     "start_time": "2022-03-10T00:44:51.086361Z"
    }
   },
   "outputs": [],
   "source": [
    "pcd_train_ndarr=calPcdByEyehand(params_eyehand_est=params_eyehand_est_lsq_,df=df_inlier)\n",
    "df_pcd_train=pd.DataFrame(pcd_train_ndarr,columns=['x','y','z'])\n",
    "df_pcd1.to_csv(f\"res/pcd/{fname_train_slim}_pcd_init_sor.csv\",index=False)\n",
    "df_lsq_res_=fitSphereR(df_pcd_train,r=sphere_r)\n",
    "df_lsq_res_"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "deaa2263",
   "metadata": {},
   "source": [
    "### display the pcd"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fdf4ed82",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-10T00:44:58.133137Z",
     "start_time": "2022-03-10T00:44:54.164100Z"
    }
   },
   "outputs": [],
   "source": [
    "pcd_train_o3d = o3d.geometry.PointCloud()\n",
    "pcd_train_o3d.points = o3d.utility.Vector3dVector(pcd_train_ndarr)\n",
    "displayO3dIns(pcd_train_o3d)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d1833cc3",
   "metadata": {},
   "source": [
    "## Verify the lds_errX_est and lds_errZ_est\n",
    "* how: if the compensated fitsphere's RMSE become smaller with our lds_errZ_est，then the estimateLDSParamsAndErrorInLdsFrame() is valid"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ed0f7b91",
   "metadata": {},
   "source": [
    "### Define utility functions"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "123aed20",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-11T23:57:07.805258Z",
     "start_time": "2022-03-11T23:57:07.796282Z"
    }
   },
   "outputs": [],
   "source": [
    "'''Estimate the LDS's parameters and the error data for calibrating the sphere'''\n",
    "def estimateLDSParamsAndErrorInLdsFrame(params_eyehand_est,df,sphere_r):\n",
    "    xyz_bh_tuple=(df.x_bh,df.y_bh,df.z_bh) #　tuple type\n",
    "    lds_meas_pt=df.loc[:,'x_s':'z_s'].to_numpy()\n",
    "\n",
    "    '''Estimate the lds_dis_est and signed lds_errZ_est'''\n",
    "    # calculate the normals in LDS_Frame\n",
    "    sphere_center_est_in_lds_CS= calSphereCenterInSensorCS(params_eyehand_est=params_eyehand_est,xyz_bh=xyz_bh_tuple)\n",
    "    print(f\"sphere_center_est_in_lds_CS:{sphere_center_est_in_lds_CS}\")\n",
    "    nxyz_lds_frame=lds_meas_pt-sphere_center_est_in_lds_CS\n",
    "    nxyz_lds_frame_unit=nxyz_lds_frame/(np.linalg.norm(x=nxyz_lds_frame,ord=2,axis=1)[:, None])     \n",
    "    \n",
    "    # calculate the ideal point, By intersect the sphere and line\n",
    "    Q1,Q2=intersectSphereAndLineVec(sphere_center_pt=sphere_center_est_in_lds_CS, sphere_r=sphere_r, lds_emit_pt=lds_emit_pt, lds_meas_pt=lds_meas_pt)\n",
    "    print(f\"Q1:{Q1}\")\n",
    "    print(f\"Q2:{Q2}\")\n",
    "    vec_PQ=Q1-lds_meas_pt\n",
    "    vec_PQ2=Q2-lds_meas_pt\n",
    "    vec_LP= lds_meas_pt-lds_emit_pt\n",
    "    print(f\"lds_meas_pt:{lds_meas_pt}\")\n",
    "    print(f\"vec_PQ:{vec_PQ}\")\n",
    "    print(f\"vec_LP:{vec_LP}\")\n",
    "    \n",
    "    # Calculate the distance\n",
    "    lds_dis_est=np.linalg.norm(vec_LP,axis=1)\n",
    "    \n",
    "    # calculate the direction\n",
    "    arccos_PQ_LP=np.sum((vec_PQ*vec_LP),axis=1)/((np.linalg.norm(x=vec_PQ,axis=1) * np.linalg.norm(x=vec_LP,axis=1))) # calculate the +/- of errZ\n",
    "    lds_errZ_est_old=-arccos_PQ_LP*np.linalg.norm(vec_PQ,axis=1) # -error, is the compensation values on Z direction    \n",
    "    sign_err=-np.sum((vec_PQ*vec_LP),axis=1)/((np.linalg.norm(x=vec_PQ,axis=1) * np.linalg.norm(x=vec_LP,axis=1))) # calculate the +/- of errZ\n",
    "    lds_errNorm_est=sign_err*np.linalg.norm(vec_PQ,axis=1) # the error on Z direction\n",
    "    lds_errNorm_est2=sign_err*np.linalg.norm(vec_PQ2,axis=1) # the error on Z direction\n",
    "    lds_errX_est=vec_PQ[:,0]\n",
    "    lds_errZ_est=vec_PQ[:,2]\n",
    "    print(f\"sign_err:{sign_err}\")\n",
    "    print(f\"lds_errX_est=vec_PQ[:,0]:{lds_errX_est}\")\n",
    "    print(f\"lds_errZ_est=vec_PQ[:,2]:{lds_errZ_est}\")\n",
    "    print(f\"lds_errZ_est_old:{lds_errZ_est_old}\")\n",
    "    print(f\"lds_errNorm_est:{lds_errNorm_est}\")\n",
    "    print(f\"lds_errNorm_est2:{lds_errNorm_est2}\")\n",
    "        \n",
    "    '''Estimate the signed lds_alpha_est'''\n",
    "    vec_CP= lds_meas_pt - sphere_center_est_in_lds_CS\n",
    "#     print(f\"vec_CP:{vec_CP.shape}\")\n",
    "    vec_CP_in_XZ=vec_CP[:,[0,2]] # get the XZ plane's projection of vec_CP\n",
    "    vec_PL_in_XZ=-vec_LP[:,[0,2]] #　the vector from measured point estimateLDSParamsAndErrorInLdsFrameto the lds's virtual emitting point\n",
    "    lds_alpha_est = cal2DSignedAngleFromV1ToV2Vec(v1=vec_PL_in_XZ,v2=vec_CP_in_XZ)\n",
    "    # TODO: lds_alpha_est must not greater than 90°    \n",
    "    \n",
    "    '''Estimate the signed lds_beta_est'''\n",
    "    # TODO: lds_beta_est must not greater than 90°角度正负问题，角度范围自动处理问题，\n",
    "#     vec_CP_in_XZ_norm=np.linalg.norm(x=vec_CP_in_XZ,axis=1)\n",
    "#     lds_beta_est= np.arctan2(vec_CP[:,1],vec_CP_in_XZ_norm) # arctan2(y-coordinates,x-coordinates)\n",
    "    lds_beta_est= np.arctan2(-vec_CP[:,1],vec_CP[:,2]) # arctan2(y-coordinates,x-coordinates)\n",
    "    return lds_dis_est,lds_alpha_est, lds_beta_est,lds_errX_est, lds_errZ_est,lds_errNorm_est,lds_errNorm_est2,sphere_center_est_in_lds_CS,nxyz_lds_frame_unit\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a3d1390d",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-10T00:44:58.506139Z",
     "start_time": "2022-03-10T00:44:58.150091Z"
    },
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "df1=df_inlier.copy(deep=True)\n",
    "x0=params_eyehand_est_lsq_.loc[\"x_hs\":\"gamma_hs\"]\n",
    "lds_dis_est,lds_alpha_est, lds_beta_est, lds_errX_est,lds_errZ_est,lds_errNorm_est,lds_errNorm_est2,sphere_center_est_in_lds_CS,nxyz_lds_frame=\\\n",
    "    estimateLDSParamsAndErrorInLdsFrame(params_eyehand_est=x0,df=df1,sphere_r=sphere_r)\n",
    "\n",
    "df1.loc[:,\"lds_dis_est\"]=lds_dis_est\n",
    "df1.loc[:,\"lds_alpha_est\"]=lds_alpha_est\n",
    "df1.loc[:,\"lds_beta_est\"]=lds_beta_est\n",
    "df1.loc[:,\"lds_errX_est\"]=lds_errX_est\n",
    "df1.loc[:,\"lds_errZ_est\"]=lds_errZ_est\n",
    "df1.loc[:,\"lds_errNorm_est\"]=lds_errNorm_est\n",
    "df1.loc[:,\"lds_errNorm_est2\"]=lds_errNorm_est2\n",
    "# df1=df1.dropna(axis=0,how='any')\n",
    "df1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f247c3e8",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-10T00:44:58.536058Z",
     "start_time": "2022-03-10T00:44:58.507162Z"
    }
   },
   "outputs": [],
   "source": [
    "df.loc[:,\"alpha_est\"]=df1.loc[:,\"lds_alpha_est\"]\n",
    "df.loc[:,\"beta_est\"]=df1.loc[:,\"lds_beta_est\"]\n",
    "df.loc[:,\"errZ_est\"]=df1.loc[:,\"lds_errZ_est\"]\n",
    "df.loc[:,\"errZnorm_est\"]=df1.loc[:,\"lds_errNorm_est\"]\n",
    "# df.loc[:,\"alpha_ER\"]=100*(df1.loc[:,\"lds_alpha_est\"]-df.loc[:,\"alpha\"])/df.loc[:,\"alpha\"]\n",
    "# df.loc[:,\"beta_ER\"]=100*(df1.loc[:,\"lds_beta_est\"]-df.loc[:,\"beta\"])/df.loc[:,\"beta\"]\n",
    "# df.loc[:,\"errZ_ER%\"]=100*(df1.loc[:,\"lds_errZ_est\"]-df.loc[:,\"errZ\"])/df.loc[:,\"errZ\"]\n",
    "df"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c78bd46d",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-10T00:44:58.540065Z",
     "start_time": "2022-03-10T00:44:58.538053Z"
    }
   },
   "outputs": [],
   "source": [
    "# df.to_excel(\"df_Err_comparision.xlsx\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f3289a97",
   "metadata": {},
   "source": [
    "### fitSphere without compensation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2c51b317",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-10T00:45:00.965349Z",
     "start_time": "2022-03-10T00:44:58.542042Z"
    }
   },
   "outputs": [],
   "source": [
    "pcd1_ndarr=calPcdByEyehand(params_eyehand_est=params_eyehand_est_lsq_,df=df_inlier)\n",
    "df_pcd1=pd.DataFrame(pcd1_ndarr,columns=['x','y','z'])\n",
    "fitSphereR(df_pcd1,r=sphere_r)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9c9c4e3f",
   "metadata": {},
   "source": [
    "### fitSphere with compensation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d7c5809b",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-10T00:45:01.007234Z",
     "start_time": "2022-03-10T00:45:00.967321Z"
    }
   },
   "outputs": [],
   "source": [
    "df2=df1.copy(True)\n",
    "df2.loc[:,\"x_s\"]=df2.loc[:,\"x_s\"]+df2.loc[:,\"lds_errX_est\"]\n",
    "df2.loc[:,\"z_s\"]=df2.loc[:,\"z_s\"]+df2.loc[:,\"lds_errZ_est\"]\n",
    "df2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "446cc3b1",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-10T00:45:01.186407Z",
     "start_time": "2022-03-10T00:45:01.008235Z"
    }
   },
   "outputs": [],
   "source": [
    "pcd1_ndarr=calPcdByEyehand(params_eyehand_est=params_eyehand_est_lsq_,df=df2)\n",
    "df_pcd1_comp=pd.DataFrame(pcd1_ndarr,columns=['x','y','z'])\n",
    "df_pcd1_comp"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d950a05e",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-10T00:45:01.346951Z",
     "start_time": "2022-03-10T00:45:01.187405Z"
    },
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "pcd1_ndarr=calPcdByEyehand(params_eyehand_est=params_eyehand_est_lsq_,df=df2)\n",
    "df_pcd1_comp=pd.DataFrame(pcd1_ndarr,columns=['x','y','z'])\n",
    "# fitSphereR(df_pcd1_comp,r=sphere_r)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4c3b8519",
   "metadata": {},
   "source": [
    "## Train the Automatic machine learning model"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9c4ea0e7",
   "metadata": {},
   "source": [
    "### Define Model secelction and hyper-parameter optimization\n",
    "we should select the model with lowset loss(fun)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "25be5d0f",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-10T00:45:01.364902Z",
     "start_time": "2022-03-10T00:45:01.348943Z"
    },
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "'''learning algorithm'''\n",
    "from sklearn.linear_model import LinearRegression,Ridge,RidgeCV\n",
    "from sklearn.preprocessing import StandardScaler\n",
    "from sklearn.pipeline import make_pipeline,Pipeline\n",
    "from sklearn.preprocessing import PolynomialFeatures\n",
    "from sklearn.neural_network import MLPRegressor\n",
    "from lightgbm import LGBMRegressor\n",
    "from sklearn.naive_bayes import GaussianNB\n",
    "from sklearn.gaussian_process import GaussianProcessRegressor\n",
    "from sklearn.gaussian_process.kernels import DotProduct, WhiteKernel\n",
    "from sklearn.svm import SVR\n",
    "from sklearn.ensemble import HistGradientBoostingRegressor\n",
    "from sklearn.metrics import make_scorer,r2_score\n",
    "# explicitly require this experimental feature\n",
    "from sklearn.experimental import enable_halving_search_cv # noqa\n",
    "# now you can import normally from model_selection,ref: https://scikit-learn.org/stable/modules/generated/sklearn.model_selection.HalvingGridSearchCV.html#sklearn.model_selection.HalvingGridSearchCV\n",
    "from sklearn.model_selection import HalvingGridSearchCV,GridSearchCV\n",
    "# https://scikit-optimize.github.io/stable/auto_examples/sklearn-gridsearchcv-replacement.html\n",
    "# https://scikit-optimize.github.io/stable/modules/generated/skopt.BayesSearchCV.html#skopt.BayesSearchCV\n",
    "from skopt import BayesSearchCV\n",
    "from sklearn.base import BaseEstimator\n",
    "from sklearn.model_selection import StratifiedKFold, KFold\n",
    "import multiprocessing\n",
    "from sklearn.multioutput import MultiOutputRegressor\n",
    "from sklearn.ensemble import BaggingRegressor\n",
    "from sklearn.tree import DecisionTreeRegressor\n",
    "from sklearn.svm import LinearSVR\n",
    "from sklearn.linear_model import ElasticNet\n",
    "'''TODO：Considering the HalvingGridSearchCV or BayesSearchCV'''\n",
    "\n",
    "'''Create a switcher class that works for any estimator'''\n",
    "class regSwitcher(BaseEstimator):\n",
    "    # https://stackoverflow.com/questions/51695322/compare-multiple-algorithms-with-sklearn-pipeline\n",
    "    def __init__(\n",
    "        self, \n",
    "        estimator = LinearRegression(),\n",
    "    ):\n",
    "        \"\"\"\n",
    "        A Custom BaseEstimator that can switch between classifiers.\n",
    "        :param estimator: sklearn object - The classifier\n",
    "        \"\"\" \n",
    "        self.estimator = estimator\n",
    "\n",
    "    def fit(self, X, y=None, **kwargs):\n",
    "        self.estimator.fit(X, y)\n",
    "        return self\n",
    "\n",
    "    def predict(self, X, y=None):\n",
    "        return self.estimator.predict(X)\n",
    "\n",
    "def fit_sphere_score_func(y, y_pred,kwargs):\n",
    "    df=kwargs['df']\n",
    "    x0=kwargs['x0']\n",
    "    groups_info=kwargs['groups_info']\n",
    "    ids=groups_info[hash(y.tobytes())]\n",
    "    df1=df.iloc[ids,:]\n",
    "    print(\"-----------------------\")\n",
    "    print(f\"ids:{ids}\")\n",
    "    print(f\"x0:{x0}\")\n",
    "    \n",
    "    #　compensate the z_s via predication lds_errZ_pred\n",
    "    print(f\"r2_score2_xs:{r2_score(y[:,0],y_pred[:,0])}\")\n",
    "    print(f\"r2_score2_zs:{r2_score(y[:,1],y_pred[:,1])}\")\n",
    "    \n",
    "    x_s_compensated=df1.x_s.to_numpy()+y_pred[:,0]\n",
    "    z_s_compensated=df1.z_s.to_numpy()+y_pred[:,1]\n",
    "    print(f\"y_pred:{y_pred.shape}\")\n",
    "    print(f\"x_s_compensated:{x_s_compensated.shape}\")\n",
    "    print(f\"z_s_compensated:{z_s_compensated.shape}\")\n",
    "    \n",
    "    # Estimate the present x0(eye-hand matrix)\n",
    "    args_train=(df1.x_bh,df1.y_bh,df1.z_bh,x_s_compensated,z_s_compensated,fit_sphere_method,sphere_r)\n",
    "    res_lsq = least_squares(fun=fun, x0=x0, args=args_train,method='trf',loss='huber')\n",
    "    print(f\"res_lsq.x:{res_lsq.x}\")\n",
    "    \n",
    "    # Fit a sphere\n",
    "    args_train=(df1.x_bh,df1.y_bh,df1.z_bh,x_s_compensated,z_s_compensated)\n",
    "    pcd_train=calPcdByEyehand(params_eyehand_est=res_lsq.x,df=args_train)\n",
    "    print(f\"pcd_train:{pcd_train.shape}\")\n",
    "    res_fit=fitSphere(df=pcd_train,r=sphere_r,n=1)\n",
    "    \n",
    "    fit_score_= res_fit.loc[0,'rmse']\n",
    "    if fit_score_ is np.nan:\n",
    "        return -1e+10\n",
    "    else:\n",
    "        return fit_score_    \n",
    "\n",
    "def ens(X,y,df,x0,i=0):\n",
    "    '''Perform the model selection and hyper-parameter optimization'''\n",
    "    pipeline = Pipeline([\n",
    "        ('poly',PolynomialFeatures()),\n",
    "        ('scaler', StandardScaler()),\n",
    "        ('reg', MultiOutputRegressor(regSwitcher())),\n",
    "        ])\n",
    "    \n",
    "    random_state=1\n",
    "    parameters = [\n",
    "        # Here you can configure the machine learning model you want to try\n",
    "        {\n",
    "            'poly__degree':[1,2,3],\n",
    "            'poly__interaction_only':[True,False],\n",
    "            'poly__include_bias':[True,False],\n",
    "            'reg__estimator':[LinearRegression()],\n",
    "            'reg__estimator__fit_intercept':[True,False]\n",
    "        },\n",
    "        {\n",
    "            'poly__degree':[1,2,3],\n",
    "            'poly__interaction_only':[True,False],\n",
    "            'poly__include_bias':[True,False],\n",
    "            'reg__estimator':[ElasticNet(random_state=0)],\n",
    "            'reg__estimator__l1_ratio':[0.1,0.5, 0.8]\n",
    "        },\n",
    "        { # the non-linear is very time-comsuming , so we select the linear kernel\n",
    "            'reg__estimator': [LinearSVR(random_state=0)], # https://scikit-learn.og/stable/modules/generated/sklearn.svm.SVR.html\n",
    "            'reg__estimator__C': [0.1, 1], # /default=1, Regularization, The strength of the regularization is inversely proportional to C\n",
    "            'reg__estimator__epsilon': [0.01, 0.1],# default=0.1, Epsilon in the epsilon-SVR model. It specifies the epsilon-tube within which no penalty is associated in the training loss function with points predicted within a distance epsilon from the actual value.\n",
    "        },\n",
    "        {\n",
    "            'reg__estimator':[MLPRegressor(random_state=0)],\n",
    "            'reg__estimator__hidden_layer_sizes':[10,50,100,200],\n",
    "            'reg__estimator__max_iter':[500,1000],            \n",
    "        },\n",
    "        {\n",
    "            'poly__degree':[1,2,3],\n",
    "            'poly__interaction_only':[True,False],\n",
    "            'poly__include_bias':[True,False],\n",
    "            'reg__estimator':[HistGradientBoostingRegressor(random_state=1)],\n",
    "            'reg__estimator__l2_regularization':[0,0.01,0.1],\n",
    "        },\n",
    "#         {\n",
    "#             'poly__degree':[3],\n",
    "#             'poly__interaction_only':[False],\n",
    "#             'poly__include_bias':[False],\n",
    "#             'reg__estimator':[HistGradientBoostingRegressor(random_state=1)],\n",
    "#             'reg__estimator__l2_regularization':[0.1],\n",
    "#         },\n",
    "    ]\n",
    "    \n",
    "    '''KFold Split the data'''\n",
    "    skf = KFold(n_splits=3)#.split(X_train,y_train)\n",
    "    '''Hash the test data for fit_sphere_score_func to locate the DataFrame'''\n",
    "    groups_info={}\n",
    "    for train, test in skf.split(X, y):\n",
    "        groups_info[hash(y[test].tobytes())]=test\n",
    "    \n",
    "    fit_sphere_scorer=make_scorer(fit_sphere_score_func,\n",
    "                          greater_is_better=False,\n",
    "                          kwargs={'df':df,'x0':x0,'groups_info':groups_info}\n",
    "                         )\n",
    "    # GridSearchCV  HalvingGridSearchCV\n",
    "    gscv = GridSearchCV(pipeline, \n",
    "                        parameters, \n",
    "                        n_jobs=1,# multiprocessing.cpu_count()\n",
    "                        scoring=fit_sphere_scorer,\n",
    "                        cv=skf,\n",
    "                        verbose=3)\n",
    "    \n",
    "    gscv.fit(X, y)    \n",
    "    print(f\"gscv.cv_results_:{gscv.cv_results_}\")\n",
    "    print(f\"gscv.best_params_:{gscv.best_params_}\")\n",
    "    print(f\"gscv.best_estimator_:{gscv.best_estimator_}\")\n",
    "    print(f\"gscv.best_score_:{gscv.best_score_}\")\n",
    "    return gscv.cv_results_,gscv.best_score_,gscv.best_estimator_,gscv.best_params_\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "97ab83c6",
   "metadata": {},
   "source": [
    "### Training the model by cross-validation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4955f94d",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-10T00:45:01.378891Z",
     "start_time": "2022-03-10T00:45:01.366925Z"
    }
   },
   "outputs": [],
   "source": [
    "'''Iteration'''\n",
    "x0=params_eyehand_est_lsq_.iloc[0:6].to_numpy()\n",
    "df1=df_inlier.copy(deep=True)\n",
    "# n_sample_train=1000000\n",
    "# df_train=df1.sample(n_sample_train)\n",
    "df_train=df1\n",
    "best_score_arr,x0_arr,shape_arr,r2_arr,score_arr,cost_arr,rmse_arr,mae_arr,shp_rmse_arr,shp_mae_arr,elapsed_time_arr=[],[],[],[],[],[],[],[],[],[],[]\n",
    "best_estimator_arr_=[]\n",
    "rmse_arr.append(df_lsq_res.loc[0,'rmse'])\n",
    "mae_arr.append(df_lsq_res.loc[0,'mae'])\n",
    "shp_rmse_arr.append(df_fit_sph_res1.loc[0,'rmse'])\n",
    "shp_mae_arr.append(df_fit_sph_res1.loc[0,'mae'])\n",
    "cv_results_,best_x0_,best_estimator_,residual=None,None,None,None\n",
    "epoch=5\n",
    "X,y=None,None"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f0666c39",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-10T01:50:25.326069Z",
     "start_time": "2022-03-10T00:45:01.380883Z"
    },
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "for i in range(epoch):\n",
    "    t1=time.time()\n",
    "    x0_arr.append(x0)\n",
    "    shape_arr.append(df1.shape[0])\n",
    "    print(f\"---------{i}th iteration--------\")\n",
    "    df1=df_inlier.copy(deep=True) # 为了让解封闭，所以每次计算前都重新回到初始数据状态\n",
    "    lds_dis_est,lds_alpha_est, lds_beta_est, lds_errX_est,lds_errZ_est,lds_errNorm_est,lds_errNorm_est2,sphere_center_est_in_lds_CS,nxyz_lds_frame=\\\n",
    "        estimateLDSParamsAndErrorInLdsFrame(params_eyehand_est=x0,df=df1,sphere_r=sphere_r)\n",
    "\n",
    "    df1.loc[:,\"lds_dis_est\"]=lds_dis_est\n",
    "    df1.loc[:,\"lds_alpha_est\"]=lds_alpha_est\n",
    "    df1.loc[:,\"lds_beta_est\"]=lds_beta_est\n",
    "    df1.loc[:,\"lds_errX_est\"]=lds_errX_est\n",
    "    df1.loc[:,\"lds_errZ_est\"]=lds_errZ_est\n",
    "    df1.loc[:,\"lds_errNorm_est\"]=lds_errNorm_est\n",
    "\n",
    "    # Delete the rows contain Nan from estimateLDSParamsAndErrorInLdsFrame()\n",
    "    print(f\"df1.shape:{df1.shape}\")\n",
    "    df1=df1.dropna(axis=0,how='any')\n",
    "    print(f\"df1.shape after dropna:{df1.shape}\")\n",
    "\n",
    "    # Learning\n",
    "    if train_col_names==\"xs_zs_alpha_beta\":\n",
    "        print(f\"train_col_names: xs_zs_alpha_beta\")\n",
    "        X=np.column_stack((df1.x_s,lds_emit_pt[2]-df1.z_s,df1.lds_alpha_est, df1.lds_beta_est))    \n",
    "    elif train_col_names==\"pid_dis_alpha_beta\":\n",
    "        print(f\"train_col_names: pid_dis_alpha_beta\")\n",
    "        X=np.column_stack((df1.pid,df1.lds_dis_est,df1.lds_alpha_est, df1.lds_beta_est))    \n",
    "    else:\n",
    "        print(f\"train_col_names: dis_alpha_beta\")\n",
    "        X=np.column_stack((df1.lds_dis_est,df1.lds_alpha_est, df1.lds_beta_est))    \n",
    "\n",
    "    print(f\"X:{X.shape}\")\n",
    "    y0=df1.lds_errX_est.to_numpy()\n",
    "    y1=df1.lds_errZ_est.to_numpy()\n",
    "    y=np.column_stack((y0,y1))\n",
    "    print(f\"y:{y.shape}\")\n",
    "    X_and_y=np.column_stack((X,y))\n",
    "    cv_results_,best_score_,best_estimator_,best_params_=ens(X,y,df=df1,x0=x0,i=0)\n",
    "    best_estimator_arr_.append(best_estimator_)\n",
    "\n",
    "    #　Compensate the z_s via estimated lds_errZ_pred\n",
    "    lds_errXZ_pred=best_estimator_.predict(X)\n",
    "    df1.x_s=df1.x_s+lds_errXZ_pred[:,0]\n",
    "    df1.z_s=df1.z_s+lds_errXZ_pred[:,1]\n",
    "\n",
    "    # Estimate the present x0(eye-hand matrix)\n",
    "    args_train=(df1.x_bh,df1.y_bh,df1.z_bh,df1.x_s,df1.z_s,fit_sphere_method,sphere_r)\n",
    "    res_lsq = least_squares(fun=fun, x0=x0, args=args_train,method='trf',loss='huber')\n",
    "\n",
    "    n_validdata=df1.z_s.shape[0]\n",
    "    cost_arr.append(res_lsq.cost)\n",
    "    residual=res_lsq.fun\n",
    "    rmse=np.sqrt(np.mean(res_lsq.fun**2))\n",
    "    mae=np.mean(np.abs(res_lsq.fun))\n",
    "    print(f\"res_lsq.cost:{res_lsq.cost}\")\n",
    "    print(f\"rsme:{rmse}\")\n",
    "    print(f\"rsme_arr:{rmse_arr}\")\n",
    "    print(f\"mae:{mae}\")\n",
    "    rmse_arr.append(rmse)\n",
    "    mae_arr.append(mae)\n",
    "    best_score_arr.append(best_score_)\n",
    "\n",
    "    # Fit a sphere\n",
    "    pcd_train=calPcdByEyehand(params_eyehand_est=res_lsq.x,df=df1)\n",
    "    df_pcd_train=pd.DataFrame(pcd_train,columns=['x','y','z'])\n",
    "    res_fit=fitSphere(df=df_pcd_train,n=1)\n",
    "    shp_rmse_arr.append(res_fit.loc[0,'rmse'])\n",
    "    shp_mae_arr.append(res_fit.loc[0,'mae'])\n",
    "\n",
    "    # Set the x0 fot next iteration\n",
    "    x0=res_lsq.x\n",
    "    best_x0_=x0\n",
    "    \n",
    "    # save data\n",
    "    now=f\"{datetime.datetime.now()}\".replace(\":\",\"-\").split(\".\")[0]\n",
    "    elapsed_time_arr.append(time.time()-t1)\n",
    "    suffix_cv=f\"{now}_no_parallel\" # parallel\n",
    "    dump(best_estimator_, f'res/train_cv/{suffix_cv}_{i}_estimator_.joblib') \n",
    "    df_cv_res=pd.DataFrame(cv_results_).sort_values(\"rank_test_score\")\n",
    "    df_cv_res.to_excel(f\"res/train_cv/{suffix_cv}_{i}_cv_results-250K.xlsx\")\n",
    "    pd.DataFrame(np.column_stack((df1.lds_errX_est,df1.lds_errZ_est)),columns=['errX','errZ']).to_csv(f\"res/train_cv/{suffix_cv}_{i}_errX_errZ.csv\")\n",
    "    \n",
    "    if len(x0_arr)>1:\n",
    "        print(f\"rmse_arr[-1]({rmse_arr[-1]})>rmse_arr[-2]({rmse_arr[-2]}):{rmse_arr[-1]>rmse_arr[-2]}\")\n",
    "        if rmse_arr[-1]>rmse_arr[-2]:\n",
    "            break\n",
    "\n",
    "if len(best_estimator_arr_)>2:\n",
    "    best_estimator_=best_estimator_arr_[-2]\n",
    "else:\n",
    "    best_estimator_=best_estimator_arr_[-1]\n",
    "    \n",
    "now=f\"{datetime.datetime.now()}\".replace(\":\",\"-\").split(\".\")[0]\n",
    "suffix_cv=f\"{now}_no_parallel\" # parallel\n",
    "pd.DataFrame(x0_arr).to_excel(f\"res/train_cv/{suffix_cv}_all_x0_arr.xlsx\")\n",
    "pd.DataFrame(elapsed_time_arr).to_excel(f\"res/train_cv/{suffix_cv}_all_elapsed_time_arr.xlsx\")\n",
    "pd.DataFrame(np.column_stack((rmse_arr,mae_arr)),columns=['rmse','mae']).to_csv(f\"res/train_cv/{suffix_cv}_all_rmse_mae_arr.csv\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bc6975af",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-10T01:50:25.360971Z",
     "start_time": "2022-03-10T01:50:25.327041Z"
    },
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "# pd.DataFrame(cv_results_).to_excel(\"cv_results.xlsx\")\n",
    "df_cv_res=pd.DataFrame(cv_results_).sort_values(\"rank_test_score\")\n",
    "df_cv_res"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "030a471d",
   "metadata": {},
   "source": [
    "### Fit the sphere  with SOR, by aboving eye-hand parameters"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "dfd526ec",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-10T01:50:27.572298Z",
     "start_time": "2022-03-10T01:50:25.361979Z"
    }
   },
   "outputs": [],
   "source": [
    "params_eyehand_est=x0\n",
    "print(f\"params_eyehand_est:{params_eyehand_est}\")\n",
    "pcd_train_ndarr=calPcdByEyehand(params_eyehand_est=params_eyehand_est,df=df_inlier)\n",
    "df_pcd_train=pd.DataFrame(pcd_train_ndarr,columns=['x','y','z'])\n",
    "fitSphereR(df_pcd_train,r=sphere_r)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3bc9beff",
   "metadata": {},
   "source": [
    "### Fit the sphere with SOR and EC, by aboving eye-hand parameters"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f62027df",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-10T01:50:29.912730Z",
     "start_time": "2022-03-10T01:50:27.574293Z"
    }
   },
   "outputs": [],
   "source": [
    "params_eyehand_est=x0\n",
    "print(f\"params_eyehand_est:{params_eyehand_est}\")\n",
    "pcd_train_ndarr=calPcdByEyehand(params_eyehand_est=params_eyehand_est,df=df1)\n",
    "df_pcd_train=pd.DataFrame(pcd_train_ndarr,columns=['x','y','z'])\n",
    "fitSphereR(df_pcd_train,r=sphere_r)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6c515a57",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-10T01:50:30.099246Z",
     "start_time": "2022-03-10T01:50:29.913710Z"
    }
   },
   "outputs": [],
   "source": [
    "# df_meas_raw1.sample(n_sample_test,random_state=0)\n",
    "df11=df1.sample(10*1000)\n",
    "pcd_train_ndarr=calPcdByEyehand(params_eyehand_est=params_eyehand_est,df=df11)\n",
    "df_pcd_train=pd.DataFrame(pcd_train_ndarr,columns=['x','y','z'])\n",
    "fitSphereR(df_pcd_train,r=sphere_r)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1d6748b1",
   "metadata": {},
   "source": [
    "## Show the training results"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "60d1468b",
   "metadata": {},
   "source": [
    "### Plots"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c901f313",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-10T10:38:46.564236Z",
     "start_time": "2022-03-10T10:38:41.071278Z"
    },
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "print(f\"rmse_arr.min:{np.min(rmse_arr)}\")\n",
    "print(f\"elapsed_time_arr:{elapsed_time_arr}\")\n",
    "print(f\"shape_arr.min:{np.min(shape_arr)}\")\n",
    "print(f\"rmse_arr:{rmse_arr}\")\n",
    "print(f\"mae_arr:{mae_arr}\")\n",
    "print(f\"shp_rmse_arr:{shp_rmse_arr}\")\n",
    "# print(f\"mae_arr:{mae_arr}\")\n",
    "print(f\"score_arr:{score_arr}\")\n",
    "print(f\"x0:{x0}\")\n",
    "print(f\"x0_arr:{x0_arr}\")\n",
    "\n",
    "px.line(best_score_arr,title=\"CV Best_score_(Higher is better)\").show()\n",
    "px.histogram(df1.lds_errX_est,title=\"errX\").show()\n",
    "px.histogram(df1.lds_errZ_est,title=\"errZ\").show()\n",
    "px.line(rmse_arr,title=\"RMSE(Lower is better)\").show()\n",
    "px.line(mae_arr,title=\"MAE(Lower is better)\").show()\n",
    "px.histogram(residual,title='Residual').show()\n",
    "\n",
    "px.line(shp_rmse_arr,title=\"Sphere Fittings' RMSE with error compensation(Lower is better)\").show()\n",
    "px.line(shp_mae_arr,title=\"Sphere Fittings' MAE with error compenstaion(Lower is better)\").show()\n",
    "\n",
    "px.line(shape_arr,title=\"Amount of valid data(Higher is better)\").show()\n",
    "px.line(score_arr,title=\"R2 of fitting the lds's error\").show()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9841d3d0",
   "metadata": {},
   "source": [
    "### PDP, Partial dependence plots\n",
    "Partial dependence plots (PDP) show the dependence between the target response and a set of input features of interest, marginalizing over the values of all other input features (the ‘complement’ features). Intuitively, we can interpret the partial dependence as the expected target response as a function of the input features of interest."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "95c2d0bf",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-10T14:52:02.751189Z",
     "start_time": "2022-03-10T14:52:02.747168Z"
    }
   },
   "outputs": [],
   "source": [
    "from sklearn.inspection import PartialDependenceDisplay # displan tht PDP data\n",
    "from sklearn.inspection import partial_dependence # get PDP data"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f78f7bf5",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-10T15:49:10.520579Z",
     "start_time": "2022-03-10T15:49:10.512601Z"
    }
   },
   "outputs": [],
   "source": [
    "if len(best_estimator_arr_)>2:\n",
    "    best_estimator_=best_estimator_arr_[-2]\n",
    "else:\n",
    "    best_estimator_=best_estimator_arr_[-1]\n",
    "    \n",
    "best_estimator_"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bdb72b46",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-10T15:49:15.062837Z",
     "start_time": "2022-03-10T15:49:15.045859Z"
    }
   },
   "outputs": [],
   "source": [
    "number_of_samples=5000\n",
    "X=np.column_stack((df1.x_s,lds_emit_pt[2]-df1.z_s,df1.lds_alpha_est, df1.lds_beta_est))    \n",
    "indices = np.random.choice(X.shape[0], number_of_samples, replace=False)\n",
    "X1=X[indices]\n",
    "X1.shape\n",
    "colnames = [\"Xs\", \"Ds\", \"Alpha\", \"Beta\"]\n",
    "df_X=pd.DataFrame(X1,columns=colnames)\n",
    "plt.style.use('science')\n",
    "plt.figure(dpi=300)\n",
    "plt.rc('font',family='Times New Roman')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "36496540",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-10T15:49:19.656641Z",
     "start_time": "2022-03-10T15:49:16.417304Z"
    },
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "%config InlineBackend.figure_format='svg'#输出矢量图设置\n",
    "features = [0,1,2,3,(0,1),(2,3)] # 第i，j个特征，及第i,j个特征\n",
    "print(\"Computing partial dependence plots...\")\n",
    "tic = time.time()\n",
    "_, ax = plt.subplots(nrows=2,ncols=3,figsize=(12, 6))\n",
    "display = PartialDependenceDisplay.from_estimator(\n",
    "    best_estimator_,\n",
    "    df_X,\n",
    "    features,\n",
    "    target=0,\n",
    "    kind=\"average\",\n",
    "    n_jobs=2,\n",
    "    grid_resolution=10,\n",
    "    ax=ax,\n",
    "#     ice_lines_kw={\"color\": \"tab:blue\", \"alpha\": 0.2, \"linewidth\": 0.5},\n",
    "    pd_line_kw={\"color\": \"#F14040\", \"linestyle\": \"-\"},\n",
    ")\n",
    "\n",
    "print(f\"done in {time.time() - tic:.3f}s\")\n",
    "display.figure_.suptitle(\n",
    "    \"Partial dependence of errX value on features\\n\"\n",
    "    \"for the Improved eye-hand calibration, with AutoML\"\n",
    ")\n",
    "display.figure_.subplots_adjust(wspace=0.2, hspace=0.2)\n",
    "fname=\"PDP-errX\"\n",
    "extnames=['png','pdf','tiff','svg']\n",
    "# for ext in extnames:\n",
    "#     plt.savefig(f\"res/fig/{fname}.{ext}\", dpi='figure', format=None, bbox_inches=None, pad_inches=0.1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6c6cba9e",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-10T15:22:59.768271Z",
     "start_time": "2022-03-10T15:22:47.983Z"
    }
   },
   "outputs": [],
   "source": [
    "%config InlineBackend.figure_format='svg'#输出矢量图设置\n",
    "features = [0,1,2,3,(0,1),(2,3)] # 第i，j个特征，及第i,j个特征\n",
    "print(\"Computing partial dependence plots...\")\n",
    "tic = time.time()\n",
    "_, ax = plt.subplots(nrows=2,ncols=3,figsize=(12, 6))\n",
    "display = PartialDependenceDisplay.from_estimator(\n",
    "    best_estimator_,\n",
    "    df_X,\n",
    "    features,\n",
    "    target=1,\n",
    "    kind=\"average\",\n",
    "    n_jobs=2,\n",
    "    grid_resolution=25,\n",
    "    pd_line_kw={\"color\": \"#F14040\", \"linestyle\": \"-\"},\n",
    "    ax=ax,\n",
    ")\n",
    "\n",
    "print(f\"done in {time.time() - tic:.3f}s\")\n",
    "display.figure_.suptitle(\n",
    "    \"Partial dependence of errZ value on features\\n\"\n",
    "    \"for the Improved eye-hand calibration, with AutoML\"\n",
    ")\n",
    "\n",
    "display.figure_.subplots_adjust(wspace=0.2, hspace=0.3)\n",
    "\n",
    "fname=\"PDP-errZ\"\n",
    "extnames=['png','pdf','tiff','svg']\n",
    "for ext in extnames:\n",
    "    plt.savefig(f\"res/fig/{fname}.{ext}\", dpi='figure', format=None, bbox_inches=None, pad_inches=0.1)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8374b749",
   "metadata": {},
   "source": [
    "### PDP and ICE, Individual conditional expectation (ICE) plot"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2bf0483e",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-10T15:03:06.738798Z",
     "start_time": "2022-03-10T15:02:59.565136Z"
    }
   },
   "outputs": [],
   "source": [
    "print(\"Computing partial dependence plots...\")\n",
    "tic = time.time()\n",
    "features=[0,1,2,3]\n",
    "_, ax = plt.subplots(nrows=2,ncols=2,figsize=(12, 10))\n",
    "display = PartialDependenceDisplay.from_estimator(\n",
    "    best_estimator_,\n",
    "    df_X,\n",
    "    features,\n",
    "    kind=\"both\",\n",
    "    target=0,\n",
    "    subsample=50,\n",
    "    n_jobs=3,\n",
    "    grid_resolution=20,\n",
    "    random_state=0,\n",
    "    ice_lines_kw={\"color\": \"#515151\", \"alpha\": 0.5, \"linewidth\": 0.8},\n",
    "    pd_line_kw={\"color\":  \"#F14040\", \"linestyle\": \"-\",\"linewidth\":2},\n",
    "    ax=ax,\n",
    ")\n",
    "\n",
    "print(f\"done in {time.time() - tic:.3f}s\")\n",
    "display.figure_.suptitle(\n",
    "    \"PDP and ICE plots of errX on features\\n\"\n",
    "    \"for the improved eye-hand calibration, with AutoML\"\n",
    ")\n",
    "display.figure_.subplots_adjust(hspace=0.2,wspace=0.2)\n",
    "fname=\"PDP-ICE-errX\"\n",
    "extnames=['png','pdf','tiff','svg']\n",
    "for ext in extnames:\n",
    "    plt.savefig(f\"res/fig/{fname}.{ext}\", dpi='figure', format=None, bbox_inches=None, pad_inches=0.1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9ce094de",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-10T15:03:54.493235Z",
     "start_time": "2022-03-10T15:03:46.614603Z"
    },
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "print(\"Computing partial dependence plots...\")\n",
    "tic = time.time()\n",
    "features=[0,1,2,3]\n",
    "_, ax = plt.subplots(nrows=2,ncols=2,figsize=(12, 10))\n",
    "display = PartialDependenceDisplay.from_estimator(\n",
    "    best_estimator_,\n",
    "    df_X,\n",
    "    features,\n",
    "    kind=\"both\",\n",
    "    target=1,\n",
    "    subsample=50,\n",
    "    n_jobs=3,\n",
    "    grid_resolution=20,\n",
    "    random_state=0,\n",
    "#     ice_lines_kw={\"color\": \"tab:blue\", \"alpha\": 0.2, \"linewidth\": 0.5},\n",
    "#     pd_line_kw={\"color\": \"tab:orange\", \"linestyle\": \"--\"},\n",
    "    \n",
    "    ice_lines_kw={\"color\": \"#515151\", \"alpha\": 0.5, \"linewidth\": 0.8},\n",
    "    pd_line_kw={\"color\":  \"#F14040\", \"linestyle\": \"-\",\"linewidth\":2},\n",
    "    ax=ax,\n",
    ")\n",
    "\n",
    "print(f\"done in {time.time() - tic:.3f}s\")\n",
    "display.figure_.suptitle(\n",
    "    \"PDP and ICE plots of of errZ on features\\n\"\n",
    "    \"for the improved eye-hand calibration, with AutoML\"\n",
    ")\n",
    "display.figure_.subplots_adjust(hspace=0.2,wspace=0.2)\n",
    "fname=\"PDP-ICE-errZ\"\n",
    "extnames=['png','pdf','tiff','svg']\n",
    "for ext in extnames:\n",
    "    plt.savefig(f\"res/fig/{fname}.{ext}\", dpi='figure', format=None, bbox_inches=None, pad_inches=0.1)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "90f0fdf4",
   "metadata": {},
   "source": [
    "# Apply the two-stage eye-hand and error model"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "62c65573",
   "metadata": {},
   "source": [
    "## Load the the test data"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "307d1884",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-11T13:37:13.612697Z",
     "start_time": "2022-03-11T13:37:13.274600Z"
    }
   },
   "outputs": [],
   "source": [
    "if len(best_estimator_arr_)>2:\n",
    "    best_estimator_=best_estimator_arr_[-2]\n",
    "else:\n",
    "    best_estimator_=best_estimator_arr_[-1]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "42c1e169",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T02:15:36.351738Z",
     "start_time": "2022-03-12T02:15:36.322816Z"
    }
   },
   "outputs": [],
   "source": [
    "best_estimator_= load(r\"res\\train_cv-250K\\2022-03-07 15-53-38_no_parallel_1_estimator_.joblib\")\n",
    "x0=np.asarray([  -80.342560508225 ,\t -15.543752033621 ,\t -61.606172672992 ,\t 1.530600966976 ,\t 0.086670906014 ,\t 1.536164497450  ])\n",
    "\n",
    "x0_lls_linear_lm=np.asarray([-80.34245759, -15.54366193, -61.60630461,   1.53055302  , 0.0866374,   1.5362051 ])\n",
    "x0_lls_linear_lm_unknownR=np.asarray([-80.3425403 , -15.57389033 ,-61.60505205 ,  4.67214613  , 3.05498474,4.67778049 ])\n",
    "x0_tls_linear_lm_unknownR=np.asarray([-8.03425436e+01, -1.55744357e+01, -6.16050345e+01 ,-4.75263057e+00,  8.66082105e-02, -4.74699597e+00 ])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3d850b73",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-11T23:59:52.080379Z",
     "start_time": "2022-03-11T23:59:51.240070Z"
    },
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "# fname_test=\"20220208/Eid7-0.5_A79.0Cd18.0Cr78.0_ACid1_12_stand_sph_R12.7nY1nZ2F1P10.xyzprepro_sub_1e6_filtered.csv\"\n",
    "# fname_test=\"20220218/verification/Eidzg3_A60.0Cd20.0Cr80.0_ACid1_1_SmallSphere_R10nZ2F1P20.xyzprepro_filtered.csv\"\n",
    "# fname_test=\"20220218night/verification/Eidzg3_A60.0Cd20.0Cr80.0_ACid1_20_BigSphere_R12.7nY1nZ2F1P20.xyzprepro_sub_1e6_filtered.csv\"\n",
    "# fname_test=\"20220218night/calibration/Eid_zg3_A60.0Cd20.0Cr80.0_ACid1_3_calibration_big_phere_R12.7nY10nZ10F1P25.xyzprepro_sub_1e6_filtered.csv\"\n",
    "# fname_test=\"20220218night/verification/\"\n",
    "# fname_test=\"20220209/verification/Eid10_A67.5Cd12.0Cr72.0_ACid1_10_stand_sph_R12.7nY1nZ2F1P10.xyzprepro_sub_1e6_filtered.csv\"\n",
    "# fname_test=\"F:/0实验数据/20220219-zg2/verification/Eidzg2_A60.0Cd10.0Cr70.0_ACid1_10_SmallSphere_R10nZ2F1P20.xyzprepro_filtered.csv\"\n",
    "# fname_test=\"F:/0实验数据/20220219-zg2/verification/Eidzg2_A60.0Cd10.0Cr70.0_ACid1_10_SmallSphere_R10nZ2F1P20.xyzprepro_filtered.csv\"\n",
    "# fname_test=\"F:/0实验数据/20220219-zg2/verification/Eidzg2_A60.0Cd10.0Cr70.0_ACid1_10_BigSphere_R12.7nY1nZ2F1P20.xyzprepro_sub_1e6_filtered.csv\"\n",
    "# fname_test=\"F:/0实验数据/20220219-zg2/verification/Eidzg2_A60.0Cd10.0Cr70.0_ACid1_10_BigSphere_R12.7nY1nZ2F1P20.xyzprepro_sub_1e6_filtered.csv\"\n",
    "# fname_test=\"20220206/verification/Eid10_A67.5Cd12.0Cr72.0_ACid1_1_check_phere_R10F1P10.xyzprepro_sub_1e6_filtered.csv\"\n",
    "# fname_test=\"20220206/verification/Eid11-0.5_A67.5Cd18.0Cr78.0_ACid1_1_check_sphere_R10F1P10.xyzprepro_sub_1e6_filtered.csv\"\n",
    "# fname_test=\"20220206/verification/Eid11-0.5_A67.5Cd18.0Cr78.0_ACid1_1_stand_sph_R12.7nY1nZ1F1P10.xyzprepro_sub_1e6_filtered.csv\"\n",
    "# fname_test=\"20220218night_zg3/calibration/Eid_zg3_A60.0Cd20.0Cr80.0_ACid1_1_calibration_big_phere_R12.7nY10nZ10F1P40.xyzprepro_sub_1e6_filtered.csv\"\n",
    "# fname_test=\"20220218night_zg3/calibration/Eid_zg3_ACid1_1_calibration_small_sphere_R10nY10nZ10F1P40.xyzprepro_sub_1e6_filtered.csv\"\n",
    "\n",
    "\n",
    "# fname_test=\"F:/0实验数据/20220227-zg1/calibration/Eid_zg1_ACid1_2_calibration_small_sphere_ScaleX22.0Y22Z30_R10nY12nZ12F1P20.xyzprepro_sub_1e6_filtered.csv\"\n",
    "# fname_test=\"F:/0实验数据/20220219-zg2/calibration/Eid_zg2_A60.0Cd10.0Cr70.0_ACid1_3_calibration_big_phere_R12.7nY10nZ10F1P25.xyzprepro_sub_1e6_filtered.csv\"\n",
    "# fname_test=\"F:/0实验数据/20220219-zg2/calibration/Eid_zg2_ACid1_2_calibration_small_sphere_R10nY10nZ10F1P20.xyzprepro_sub_1e6_filtered.csv\"\n",
    "# fname_test=\"F:/0实验数据/20220220-zg3/calibration/Eid_zg3_A60.0Cd20.0Cr80.0_ACid1_1_calibration_big_phere_R12.7nY10nZ10F1P25.xyzprepro_sub_1e6_filtered.csv\"\n",
    "# fname_test=\"F:/0实验数据/20220220-zg3/calibration/Eid_zg3_A60.0Cd20.0Cr80.0_ACid1_3_calibration_big_phere_R12.7nY10nZ10F1P25.xyzprepro_sub_1e6_filtered.csv\"\n",
    "\n",
    "# fname_test=\"F:/0实验数据/20220220-zg3/calibration/Eid_zg3_ACid1_2_calibration_small_sphere_ScaleX18.0Y24Z30_R10nY10nZ10F1P20.xyzprepro_sub_1e6_filtered.csv\"\n",
    "# fname_test=\"F:/0实验数据/20220219-zg2/verification/Eidzg2_A60.0Cd10.0Cr70.0_ACid1_10_BigSphere_R12.7nY1nZ2F1P20.xyzprepro_sub_1e6_filtered.csv\"\n",
    "# fname_test=\"F:/0实验数据/20220220-zg3/verification/Eidzg3_A60.0Cd20.0Cr80.0_ACid1_10_BigSphere_R12.7nY1nZ2F1P20.xyzprepro_sub_1e6_filtered.csv\"\n",
    "\n",
    "fname_test=\"data/Eid_zg1_A60.0Cd0.0Cr60.0_ACid2_1_calibration_big_phere_R12.7nY10nZ10F1P25.xyzprepro_sub_1e6_filtered.csv\"\n",
    "\n",
    "fit_sphere_method=\"TLS\"\n",
    "# fit_sphere_method=\"LLS\"\n",
    "fname_test_slim=f\"train_zg2_small_test_{fit_sphere_method}_zg3_ACid1_2_big\"\n",
    "if (\"BigSphere\" in fname_test) or (\"_big_phere_\" in fname_test):\n",
    "    r_test=big_sphere_r\n",
    "else:\n",
    "    r_test=small_sphere_r\n",
    "# fname_test=\"20220204home/Eid10_A67.5Cd12.0Cr72.0_ACid1_1_check_sphere_R10F1P10.xyzprepro_sub_1e6_filted.csv\"\n",
    "# fname_test=\"20220206/Eid11-0.5_A67.5Cd18.0Cr78.0_ACid1_2_check_sphere_R10F1P10.xyzprepro_sub_1e6_filtered.csv\"\n",
    "# r_test=big_sphere_r\n",
    "df_meas_raw0=pd.read_csv(fname_test,index_col=0)\n",
    "df_meas_raw0.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9b4852aa",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-11T23:59:53.309080Z",
     "start_time": "2022-03-11T23:59:53.296114Z"
    }
   },
   "outputs": [],
   "source": [
    "df_meas_raw0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cc532f42",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-11T23:59:56.004044Z",
     "start_time": "2022-03-11T23:59:56.000056Z"
    }
   },
   "outputs": [],
   "source": [
    "# df_meas_raw1=df_meas_raw0[(df_meas_raw0['z_bh']>60) & (df_meas_raw0['z_bh']<65)]\n",
    "# df_meas_raw1=df_meas_raw0[(df_meas_raw0['x_bh']>44)]\n",
    "df_meas_raw1=df_meas_raw0"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "69aae1b5",
   "metadata": {},
   "source": [
    "## Down sample"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "76ccabb0",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T00:00:00.152310Z",
     "start_time": "2022-03-11T23:59:58.302462Z"
    }
   },
   "outputs": [],
   "source": [
    "n_sample_test=100*1000\n",
    "fname_test_slim=f\"{fname_test_slim}_{n_sample_test}\"\n",
    "if df_meas_raw1.shape[0]<n_sample_test:\n",
    "    df_meas_raw=df_meas_raw1\n",
    "else:\n",
    "    df_meas_raw=df_meas_raw1.sample(n_sample_test,random_state=0)\n",
    "df_meas_raw=df_meas_raw.reset_index() # For SOR\n",
    "px.scatter(df_meas_raw,x='x_s',y='z_s').show()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "36a7e315",
   "metadata": {},
   "source": [
    "## SOR and fit"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6cc5e556",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T00:00:02.069034Z",
     "start_time": "2022-03-12T00:00:02.064040Z"
    }
   },
   "outputs": [],
   "source": [
    "fname_test_slim"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e5961d7d",
   "metadata": {},
   "source": [
    "###  Fit without SOR"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8c6ace3b",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T00:00:03.203958Z",
     "start_time": "2022-03-12T00:00:02.070019Z"
    }
   },
   "outputs": [],
   "source": [
    "# params_eyehand_est=params_eyehand_est_lsq_\n",
    "params_eyehand_est=x0\n",
    "pcd_raw_ndarr=calPcdByEyehand(params_eyehand_est=params_eyehand_est,df=df_meas_raw)\n",
    "df_pcd_raw=pd.DataFrame(pcd_raw_ndarr,columns=['x','y','z'])\n",
    "\n",
    "fit_res_noSOR_noEC_r=fitSphereR(df_pcd_raw,r=r_test)\n",
    "fit_res_noSOR_noEC_r"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "014c35be",
   "metadata": {},
   "source": [
    "### SOR"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4981d1e4",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T00:00:18.695843Z",
     "start_time": "2022-03-12T00:00:03.204955Z"
    }
   },
   "outputs": [],
   "source": [
    "pcd_o3d = o3d.geometry.PointCloud()\n",
    "pcd_o3d.points = o3d.utility.Vector3dVector(pcd_raw_ndarr)\n",
    "\n",
    "# '''Estimate normals with respect to consistent tangent planes'''\n",
    "pcd_o3d.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=2, max_nn=300))  \n",
    "pcd_o3d.orient_normals_consistent_tangent_plane(k=20) # orient the normals with respect to consistent tangent planes\n",
    "xyz_nxyz=np.column_stack((np.asarray(pcd_o3d.points),np.asarray(pcd_o3d.normals)))\n",
    "df_meas=df_meas_raw.copy(True)\n",
    "df_meas.loc[:,[\"x\",'y','z','nx','ny','nz']]=xyz_nxyz\n",
    "\n",
    "# '''SOR'''\n",
    "pcd_o3d_inlier,ind=removeStatisticalOutlier3d(pcd_o3d,nb_neighbors=30,std_ratio=5,is_show=1)\n",
    "df_meas_inlier=df_meas.iloc[ind]\n",
    "# df_meas_inlier=df_meas # Do not SOR\n",
    "\n",
    "'''Save Data'''\n",
    "displayO3dIns(pcd_o3d_inlier,is_show_normal=1)\n",
    "df_meas_inlier"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "cd2b3f1d",
   "metadata": {},
   "source": [
    "### Fit by traditional methods"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "32faa99a",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T00:11:29.205023Z",
     "start_time": "2022-03-12T00:11:28.325387Z"
    }
   },
   "outputs": [],
   "source": [
    "'''traditional methods'''\n",
    "params_eyehand_est=params_eyehand_est_lsq_\n",
    "pcd_tranditional=calPcdByEyehand(params_eyehand_est=params_eyehand_est,df=df_meas_inlier)\n",
    "df_pcd_tranditional=pd.DataFrame(pcd_tranditional,columns=['x','y','z'])\n",
    "fit_res_isSOR_noEC_tranditional_r=fitSphereR(pcd_tranditional,r=r_test)\n",
    "fit_res_isSOR_noEC_tranditional_r"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "313b39b1",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T00:11:29.608953Z",
     "start_time": "2022-03-12T00:11:29.206027Z"
    }
   },
   "outputs": [],
   "source": [
    "df_pcd_tranditional.to_csv(f\"res/pcd/{fname_test_slim}_traditional.csv\",index=False)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d4b67965",
   "metadata": {},
   "source": [
    "### Fit by Our methods without error compensation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7f69f98a",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T00:11:30.679094Z",
     "start_time": "2022-03-12T00:11:29.609921Z"
    }
   },
   "outputs": [],
   "source": [
    "params_eyehand_est=x0\n",
    "print(f\"params_eyehand_est:{params_eyehand_est}\")\n",
    "pcd_train_ndarr=calPcdByEyehand(params_eyehand_est=params_eyehand_est,df=df_meas_inlier)\n",
    "# df_pcd_train=pd.DataFrame(pcd_train_ndarr,columns=['x','y','z'])\n",
    "fit_res_isSOR_noEC_r=fitSphereR(pcd_train_ndarr,r=sphere_r)\n",
    "fit_res_isSOR_noEC_r"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3164a7c2",
   "metadata": {},
   "source": [
    "## Prepare data for error compensation\n",
    "* SOR: Remove Statistical Outlier\n",
    "* EC: Error Compensation"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8cbcd638",
   "metadata": {},
   "source": [
    "## Compensate the pcd"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0ff817f7",
   "metadata": {},
   "source": [
    "### Analytical Estimate the nomals(Only for big sphere)\n",
    "* 如果出现nan，那么就要看params_eyehand_est是不是没弄对~！\n",
    "* 球心换算是不是没弄对estimateLDSParamsAndErrorInLdsFrame\n",
    "* 球心换算算法好像默认球心坐标为0了"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "63c91955",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T00:11:30.875538Z",
     "start_time": "2022-03-12T00:11:30.680085Z"
    }
   },
   "outputs": [],
   "source": [
    "df_meas_inlier_analy=df_meas_inlier.copy(deep=1)\n",
    "# print(df_meas_inlier_analy)\n",
    "\n",
    "# params_eyehand_est=df_lsq_res.iloc[0,0:6] \n",
    "lds_dis_est,lds_alpha_est, lds_beta_est, lds_errX_est,lds_errZ_est,lds_errNorm_est,lds_errNorm_est2,sphere_center_est_in_lds_CS,nxyz_lds_frame=\\\n",
    "    estimateLDSParamsAndErrorInLdsFrame(params_eyehand_est=x0,df=df_meas_inlier_analy,sphere_r=r_test)\n",
    "\n",
    "df_meas_inlier_analy.loc[:,[\"nx_lds\",'ny_lds','nz_lds']]=nxyz_lds_frame\n",
    "df_meas_inlier_analy.loc[:,\"lds_dis_est\"]=lds_dis_est\n",
    "df_meas_inlier_analy.loc[:,\"lds_alpha_est\"]=lds_alpha_est\n",
    "df_meas_inlier_analy.loc[:,\"lds_beta_est\"]=lds_beta_est\n",
    "df_meas_inlier_analy.loc[:,\"lds_errX_est\"]=lds_errX_est\n",
    "df_meas_inlier_analy.loc[:,\"lds_errZ_est\"]=lds_errZ_est\n",
    "\n",
    "# delete the rows contain Nan from estimateLDSParamsAndErrorInLdsFrame()\n",
    "print(f\"df_meas_inlier_analy.shape:{df_meas_inlier_analy.shape}\")\n",
    "# df_meas_inlier_analy=df_meas_inlier_analy.dropna(axis=0,how='any')\n",
    "# print(f\"df_meas_inlier_analy.shape after dropna:{df_meas_inlier_analy.shape}\")\n",
    "df_meas_inlier_analy"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "49feb30c",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T00:11:32.222935Z",
     "start_time": "2022-03-12T00:11:30.876535Z"
    }
   },
   "outputs": [],
   "source": [
    "'''compensate'''\n",
    "X_test=df_meas_inlier_analy.loc[:,['x_s','z_s','lds_alpha_est','lds_beta_est']]\n",
    "X_test.loc[:,'z_s']=lds_emit_pt[2]-X_test.loc[:,'z_s']\n",
    "lds_errXZ_pred_for_compensation=best_estimator_.predict(X_test)\n",
    "df_meas_inlier_analy_comp=df_meas_inlier_analy.copy(deep=True)\n",
    "print(df_meas_inlier_analy_comp.shape)\n",
    "print(lds_errXZ_pred_for_compensation.shape)\n",
    "df_meas_inlier_analy_comp.loc[:,'lds_errX_est']=lds_errXZ_pred_for_compensation[:,0]\n",
    "df_meas_inlier_analy_comp.loc[:,'lds_errZ_est']=lds_errXZ_pred_for_compensation[:,1]\n",
    "df_meas_inlier_analy_comp.x_s=df_meas_inlier_analy_comp.x_s+lds_errXZ_pred_for_compensation[:,0]\n",
    "df_meas_inlier_analy_comp.z_s=df_meas_inlier_analy_comp.z_s+lds_errXZ_pred_for_compensation[:,1]\n",
    "# df_meas_inlier_analy_comp.x_s=df_meas_inlier_analy_comp.x_s+df_meas_inlier_analy_comp.lds_errX_est\n",
    "# df_meas_inlier_analy_comp.z_s=df_meas_inlier_analy_comp.z_s+df_meas_inlier_analy_comp.lds_errZ_est\n",
    "df_meas_inlier_analy_comp=df_meas_inlier_analy_comp.dropna(axis=0,how='any')\n",
    "\n",
    "params_eyehand_est=x0\n",
    "pcd_meas_inlier_analy_comp=calPcdByEyehand(params_eyehand_est=params_eyehand_est,df=df_meas_inlier_analy_comp)\n",
    "print(pcd_meas_inlier_analy_comp.shape)\n",
    "fit_res_isSOR_isEC_r_analy=fitSphereR(df=pcd_meas_inlier_analy_comp,n=5,r=r_test)\n",
    "fit_res_isSOR_isEC_r_analy"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "da63339a",
   "metadata": {},
   "source": [
    "### Estimate the d,alpha and beta of each point in pcd"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e7fe9f8b",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T00:11:37.663387Z",
     "start_time": "2022-03-12T00:11:32.225926Z"
    }
   },
   "outputs": [],
   "source": [
    "pcd_o3d = o3d.geometry.PointCloud()\n",
    "pcd_o3d.points = o3d.utility.Vector3dVector(pcd_raw_ndarr)\n",
    "\n",
    "'''Estimate normals with respect to consistent tangent planes'''\n",
    "pcd_o3d.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(\n",
    "    radius=1, max_nn=500))  \n",
    "pcd_o3d.orient_normals_consistent_tangent_plane(k=20) # orient the normals with respect to consistent tangent planes\n",
    "xyz_nxyz=np.column_stack((np.asarray(pcd_o3d.points),np.asarray(pcd_o3d.normals)))\n",
    "df_meas=df_meas_raw.copy(True)\n",
    "df_meas.loc[:,[\"x\",'y','z','nx','ny','nz']]=xyz_nxyz\n",
    "\n",
    "'''SOR'''\n",
    "# pcd_o3d_inlier,ind=removeStatisticalOutlier3d(pcd_o3d,nb_neighbors=20,std_ratio=2.0,is_show=0)\n",
    "# df_meas_inlier=df_meas.iloc[ind]\n",
    "# df_meas_inlier=df_meas\n",
    "\n",
    "'''Display Data'''\n",
    "# displayO3dIns(pcd_o3d_inlier,is_show_normal=1)\n",
    "#         df_meas_inlier\n",
    "\n",
    "'''Estimate the dis,alpha and beta'''\n",
    "df_meas_inlier_knn=df_meas_inlier.copy(deep=True)\n",
    "normal_dis_alpha_beta_lds=estimateDisAndAlphaAndBetaInLdsFrame(df_meas_inlier_knn,params_eyehand_est=params_eyehand_est)\n",
    "print(f\"normal_dis_alpha_beta_lds:{normal_dis_alpha_beta_lds}\")\n",
    "df_meas_inlier_knn.loc[:,[\"nx_lds\",'ny_lds','nz_lds','lds_dis_est','lds_alpha_est','lds_beta_est']]=normal_dis_alpha_beta_lds\n",
    "# df_meas_inlier_knn.loc[:,'lds_alpha_est']=df_meas_inlier_knn.loc[:,'lds_alpha_est']# -np.pi\n",
    "df_meas_inlier_knn\n",
    "print(df_meas_inlier_knn.shape)\n",
    "\n",
    "'''compensate'''\n",
    "if train_col_names==\"xs_zs_alpha_beta\":\n",
    "    print(f\"train_col_names: xs_zs_alpha_beta\")\n",
    "    X_test=df_meas_inlier_knn.loc[:,['x_s','z_s','lds_alpha_est','lds_beta_est']]\n",
    "elif train_col_names==\"pid_dis_alpha_beta\":\n",
    "    print(f\"train_col_names: pid_dis_alpha_beta\")\n",
    "    X_test=df_meas_inlier_knn.loc[:,['pid','lds_dis_est','lds_alpha_est','lds_beta_est']]\n",
    "else:\n",
    "    print(f\"train_col_names: dis_alpha_beta\")\n",
    "    X_test=df_meas_inlier_knn.loc[:,['lds_dis_est','lds_alpha_est','lds_beta_est']]\n",
    "X_test.loc[:,'z_s']=lds_emit_pt[2]-X_test.loc[:,'z_s']\n",
    "lds_errXZ_pred_for_compensation=best_estimator_.predict(X_test)\n",
    "df_meas_inlier_knn_comp=df_meas_inlier_knn.copy(deep=True)\n",
    "print(df_meas_inlier_knn_comp.shape)\n",
    "print(lds_errXZ_pred_for_compensation.shape)\n",
    "df_meas_inlier_knn_comp.loc[:,'lds_errX_est']=lds_errXZ_pred_for_compensation[:,0]\n",
    "df_meas_inlier_knn_comp.loc[:,'lds_errZ_est']=lds_errXZ_pred_for_compensation[:,1]\n",
    "df_meas_inlier_knn_comp.x_s=df_meas_inlier_knn_comp.x_s+lds_errXZ_pred_for_compensation[:,0]\n",
    "df_meas_inlier_knn_comp.z_s=df_meas_inlier_knn_comp.z_s+lds_errXZ_pred_for_compensation[:,1]\n",
    "df_meas_inlier_knn_comp\n",
    "# df_meas_inlier_knn_comp\n",
    "# pcd_meas_inlier_knn_comp=calPcdByEyehand(params_eyehand_est=params_eyehand_est,df=df_meas_inlier_knn_comp)\n",
    "# pcd_meas_inlier_knn_comp"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c855643d",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T00:11:37.677350Z",
     "start_time": "2022-03-12T00:11:37.664385Z"
    }
   },
   "outputs": [],
   "source": [
    "df_meas_raw0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f9d668d0",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T00:11:37.721233Z",
     "start_time": "2022-03-12T00:11:37.679345Z"
    }
   },
   "outputs": [],
   "source": [
    "df_meas_inlier_analy"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1b44d023",
   "metadata": {},
   "source": [
    "### Fit the sphere with SOR, with EC\n",
    "* 结果不好的时候看一下r_test是不是没设置对"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "90769e80",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T00:11:38.636814Z",
     "start_time": "2022-03-12T00:11:37.723228Z"
    },
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "params_eyehand_est=x0\n",
    "# params_eyehand_est=params_eyehand_est_lsq_\n",
    "pcd_meas_inlier_knn_comp=calPcdByEyehand(params_eyehand_est=params_eyehand_est,df=df_meas_inlier_knn_comp)\n",
    "df_pcd_meas_inlier_knn_comp=pd.DataFrame(pcd_meas_inlier_knn_comp,columns=['x','y','z'])\n",
    "fit_res_isSOR_isEC_r=fitSphereR(df=pcd_meas_inlier_knn_comp,n=5,r=r_test)\n",
    "fit_res_isSOR_isEC_r"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "72d014f3",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T00:11:39.039752Z",
     "start_time": "2022-03-12T00:11:38.637797Z"
    }
   },
   "outputs": [],
   "source": [
    "df_pcd_meas_inlier_knn_comp.to_csv(f\"res/pcd/{fname_test_slim}_hybrid_ec.csv\",index=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e8299226",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T00:11:39.045738Z",
     "start_time": "2022-03-12T00:11:39.040705Z"
    }
   },
   "outputs": [],
   "source": [
    "pcd_meas_inlier_knn_comp"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "26d8cb90",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-02-28T11:17:55.870579Z",
     "start_time": "2022-02-28T11:17:55.847639Z"
    }
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ece2c4ad",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T00:11:39.051702Z",
     "start_time": "2022-03-12T00:11:39.046715Z"
    }
   },
   "outputs": [],
   "source": [
    "# fitSphereR(df=df11,n=5,r=r_test)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b7ed0dd7",
   "metadata": {},
   "source": [
    "# Conclusion"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f3a5c8b0",
   "metadata": {},
   "source": [
    "## results"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2a8df22e",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T00:11:39.057684Z",
     "start_time": "2022-03-12T00:11:39.052703Z"
    }
   },
   "outputs": [],
   "source": [
    "fname_test_slim # if the training if based on TLS, the test should be TLS."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c60dd83d",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T00:11:39.067670Z",
     "start_time": "2022-03-12T00:11:39.058686Z"
    }
   },
   "outputs": [],
   "source": [
    "fit_res_noSOR_noEC_r"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0dd42cf1",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T00:11:39.079627Z",
     "start_time": "2022-03-12T00:11:39.068659Z"
    }
   },
   "outputs": [],
   "source": [
    "fit_res_isSOR_noEC_tranditional_r"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f8a79bd1",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T00:11:39.089604Z",
     "start_time": "2022-03-12T00:11:39.081622Z"
    }
   },
   "outputs": [],
   "source": [
    "'''With Iteration, with SOR,no EC'''\n",
    "fit_res_isSOR_noEC_r"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f9670557",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T00:11:39.101542Z",
     "start_time": "2022-03-12T00:11:39.090574Z"
    }
   },
   "outputs": [],
   "source": [
    "'''With Iteration, with SOR, with EC'''\n",
    "fit_res_isSOR_isEC_r"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4eb5d86b",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T00:11:39.113514Z",
     "start_time": "2022-03-12T00:11:39.103537Z"
    }
   },
   "outputs": [],
   "source": [
    "'''With Iteration, with SOR, No EC, Analy estimate the normals'''\n",
    "fit_res_isSOR_isEC_r_analy"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b91caca6",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T00:15:52.501302Z",
     "start_time": "2022-03-12T00:15:52.471416Z"
    }
   },
   "outputs": [],
   "source": [
    "a0=fit_res_noSOR_noEC_r.loc[0,\"r\":\"err_r\"].to_numpy()\n",
    "a1=fit_res_isSOR_noEC_tranditional_r.loc[0,\"r\":\"err_r\"].to_numpy()\n",
    "a2=fit_res_isSOR_noEC_r.loc[0,\"r\":\"err_r\"].to_numpy()\n",
    "a3=fit_res_isSOR_isEC_r.loc[0,\"r\":\"err_r\"].to_numpy()\n",
    "a4=fit_res_isSOR_isEC_r_analy.loc[0,\"r\":\"err_r\"].to_numpy()\n",
    "a=list(np.row_stack((a0,a1,a2,a3,a4)).flatten())\n",
    "isok=[a3[1]<a1[1],abs(a3[2])<abs(a1[2])]\n",
    "a=a+isok\n",
    "list_a=list(map(str, a))\n",
    "res=\"\\t\".join(list_a)\n",
    "pyperclip.copy(res)\n",
    "print(res)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "df5f6c34",
   "metadata": {},
   "source": [
    "## select the parameters of estimateNormals"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0407d7eb",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2022-03-12T00:11:39.154401Z",
     "start_time": "2022-03-12T00:11:39.143431Z"
    },
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "radius_list=[1,2,3,4,5,6,7,8,9,10]\n",
    "max_nn_list=[30,50,100,250,500,1000,2000]\n",
    "if 0:\n",
    "    for radius in radius_list:\n",
    "        for max_nn in max_nn_list:\n",
    "            pcd_o3d = o3d.geometry.PointCloud()\n",
    "            pcd_o3d.points = o3d.utility.Vector3dVector(pcd_raw_ndarr)\n",
    "\n",
    "            '''Estimate normals with respect to consistent tangent planes'''\n",
    "            pcd_o3d.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(\n",
    "                radius=radius, max_nn=max_nn))  \n",
    "            pcd_o3d.orient_normals_consistent_tangent_plane(k=20) # orient the normals with respect to consistent tangent planes\n",
    "            xyz_nxyz=np.column_stack((np.asarray(pcd_o3d.points),np.asarray(pcd_o3d.normals)))\n",
    "            df_meas=df_meas_raw.copy(True)\n",
    "            df_meas.loc[:,[\"x\",'y','z','nx','ny','nz']]=xyz_nxyz\n",
    "\n",
    "            '''SOR'''\n",
    "            # pcd_o3d_inlier,ind=removeStatisticalOutlier3d(pcd_o3d,nb_neighbors=20,std_ratio=2.0,is_show=0)\n",
    "            # df_meas_inlier=df_meas.iloc[ind]\n",
    "            df_meas_inlier=df_meas\n",
    "\n",
    "            '''Display Data'''\n",
    "            # displayO3dIns(pcd_o3d_inlier,is_show_normal=1)\n",
    "            #         df_meas_inlier\n",
    "\n",
    "            '''Estimate the dis,alpha and beta'''\n",
    "            df_meas_inlier_knn=df_meas_inlier.copy(deep=True)\n",
    "            normal_dis_alpha_beta_lds=estimateDisAndAlphaAndBetaInLdsFrame(df_meas_inlier_knn,params_eyehand_est=params_eyehand_est)\n",
    "            #         print(normal_dis_alpha_beta_lds)\n",
    "            df_meas_inlier_knn.loc[:,[\"nx_lds\",'ny_lds','nz_lds','lds_dis_est','lds_alpha_est','lds_beta_est']]=normal_dis_alpha_beta_lds\n",
    "            # df_meas_inlier_knn.loc[:,'lds_alpha_est']=df_meas_inlier_knn.loc[:,'lds_alpha_est']# -np.pi\n",
    "            df_meas_inlier_knn\n",
    "            print(df_meas_inlier_knn.shape)\n",
    "\n",
    "            '''compensate'''\n",
    "            if train_col_names==\"xs_zs_alpha_beta\":\n",
    "                print(f\"train_col_names: xs_zs_alpha_beta\")\n",
    "                X_test=df_meas_inlier_knn.loc[:,['x_s','z_s','lds_alpha_est','lds_beta_est']]\n",
    "            elif train_col_names==\"pid_dis_alpha_beta\":\n",
    "                print(f\"train_col_names: pid_dis_alpha_beta\")\n",
    "                X_test=df_meas_inlier_knn.loc[:,['pid','lds_dis_est','lds_alpha_est','lds_beta_est']]\n",
    "            else:\n",
    "                print(f\"train_col_names: dis_alpha_beta\")\n",
    "                X_test=df_meas_inlier_knn.loc[:,['lds_dis_est','lds_alpha_est','lds_beta_est']]\n",
    "            X_test.loc[:,'z_s']=lds_emit_pt[2]-X_test.loc[:,'z_s']\n",
    "            lds_errXZ_pred_for_compensation=best_estimator_.predict(X_test)\n",
    "            df_meas_inlier_knn_comp=df_meas_inlier_knn.copy(deep=True)\n",
    "            print(df_meas_inlier_knn_comp.shape)\n",
    "            print(lds_errXZ_pred_for_compensation.shape)\n",
    "            df_meas_inlier_knn_comp.loc[:,'lds_errX_est']=lds_errXZ_pred_for_compensation[:,0]\n",
    "            df_meas_inlier_knn_comp.loc[:,'lds_errZ_est']=lds_errXZ_pred_for_compensation[:,1]\n",
    "            df_meas_inlier_knn_comp.x_s=df_meas_inlier_knn_comp.x_s+lds_errXZ_pred_for_compensation[:,0]\n",
    "            df_meas_inlier_knn_comp.z_s=df_meas_inlier_knn_comp.z_s+lds_errXZ_pred_for_compensation[:,1]\n",
    "            pcd_meas_inlier_knn_comp=calPcdByEyehand(params_eyehand_est=params_eyehand_est,df=df_meas_inlier_knn_comp)\n",
    "            fit_res_isSOR_isEC_r=fitSphereR(df=pcd_meas_inlier_knn_comp,n=5,r=r_test)\n",
    "            print(f\"-------radius:{radius},max_nn:{max_nn}\")\n",
    "            print(fit_res_isSOR_isEC_r)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f67a13da",
   "metadata": {},
   "source": [
    "## end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "39229394",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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.3"
  },
  "toc": {
   "base_numbering": 1,
   "nav_menu": {
    "height": "672px",
    "width": "438px"
   },
   "number_sections": true,
   "sideBar": true,
   "skip_h1_title": false,
   "title_cell": "Table of Contents",
   "title_sidebar": "Contents",
   "toc_cell": false,
   "toc_position": {
    "height": "1574px",
    "left": "1609.62px",
    "top": "178.125px",
    "width": "287.375px"
   },
   "toc_section_display": true,
   "toc_window_display": true
  },
  "varInspector": {
   "cols": {
    "lenName": 16,
    "lenType": 16,
    "lenVar": 40
   },
   "kernels_config": {
    "python": {
     "delete_cmd_postfix": "",
     "delete_cmd_prefix": "del ",
     "library": "var_list.py",
     "varRefreshCmd": "print(var_dic_list())"
    },
    "r": {
     "delete_cmd_postfix": ") ",
     "delete_cmd_prefix": "rm(",
     "library": "var_list.r",
     "varRefreshCmd": "cat(var_dic_list()) "
    }
   },
   "oldHeight": 661,
   "position": {
    "height": "683px",
    "left": "763px",
    "right": "20px",
    "top": "120px",
    "width": "250px"
   },
   "types_to_exclude": [
    "module",
    "function",
    "builtin_function_or_method",
    "instance",
    "_Feature"
   ],
   "varInspector_section_display": "block",
   "window_display": false
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}