2-1单个点云测试.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "def inverse_rigid_trans(Tr):\n",
    "    ''' Inverse a rigid body transform matrix (3x4 as [R|t])\n",
    "        [R'|-R't; 0|1]\n",
    "    '''\n",
    "    inv_Tr = np.zeros_like(Tr)  # 3x4\n",
    "    inv_Tr[0:3, 0:3] = np.transpose(Tr[0:3, 0:3])\n",
    "    inv_Tr[0:3, 3] = np.dot(-np.transpose(Tr[0:3, 0:3]), Tr[0:3, 3])\n",
    "    return inv_Tr"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "class Calibration(object):\n",
    "    ''' Calibration matrices and utils\n",
    "        3d XYZ in <label>.txt are in rect camera coord.\n",
    "        2d box xy are in image2 coord\n",
    "        Points in <lidar>.bin are in Velodyne coord.\n",
    "\n",
    "        y_image2 = P^2_rect * x_rect\n",
    "        y_image2 = P^2_rect * R0_rect * Tr_velo_to_cam * x_velo\n",
    "        x_ref = Tr_velo_to_cam * x_velo\n",
    "        x_rect = R0_rect * x_ref\n",
    "\n",
    "        P^2_rect = [f^2_u,  0,      c^2_u,  -f^2_u b^2_x;\n",
    "                    0,      f^2_v,  c^2_v,  -f^2_v b^2_y;\n",
    "                    0,      0,      1,      0]\n",
    "                 = K * [1|t]\n",
    "\n",
    "        image2 coord:\n",
    "         ----> x-axis (u)\n",
    "        |\n",
    "        |\n",
    "        v y-axis (v)\n",
    "\n",
    "        velodyne coord:\n",
    "        front x, left y, up z\n",
    "\n",
    "        rect/ref camera coord:\n",
    "        right x, down y, front z\n",
    "\n",
    "        Ref (KITTI paper): http://www.cvlibs.net/publications/Geiger2013IJRR.pdf\n",
    "\n",
    "        TODO(rqi): do matrix multiplication only once for each projection.\n",
    "    '''\n",
    "\n",
    "    def __init__(self, calib_filepath):\n",
    "        calibs = self.read_calib_file(calib_filepath)\n",
    "        # Rigid transform from Velodyne coord to reference camera coord\n",
    "        self.V2C = calibs['Tr_velo2cam']\n",
    "        self.V2C = np.reshape(self.V2C, [3, 4])\n",
    "        self.C2V = inverse_rigid_trans(self.V2C)\n",
    "        # Rotation from reference camera coord to rect camera coord\n",
    "        self.R0 = calibs['R_rect']\n",
    "        self.R0 = np.reshape(self.R0, [3, 3])\n",
    "        # Projection matrix from rect camera coord to image2 coord\n",
    "        self.P = calibs['P2']\n",
    "        self.P = np.reshape(self.P, [3, 4])\n",
    "\n",
    "        # Camera intrinsics and extrinsics\n",
    "        self.c_u = self.P[0, 2]\n",
    "        self.c_v = self.P[1, 2]\n",
    "        self.f_u = self.P[0, 0]\n",
    "        self.f_v = self.P[1, 1]\n",
    "        self.b_x = self.P[0, 3] / (-self.f_u)  # relative\n",
    "        self.b_y = self.P[1, 3] / (-self.f_v)\n",
    "\n",
    "    def read_calib_file(self, filepath):\n",
    "        with open(filepath) as f:\n",
    "            lines = f.readlines()\n",
    "\n",
    "        obj = lines[2].strip().split(' ')[1:]\n",
    "        P2 = np.array(obj, dtype=np.float32)\n",
    "        obj = lines[3].strip().split(' ')[1:]\n",
    "        P3 = np.array(obj, dtype=np.float32)\n",
    "        obj = lines[4].strip().split(' ')[1:]\n",
    "        R0 = np.array(obj, dtype=np.float32)\n",
    "        obj = lines[5].strip().split(' ')[1:]\n",
    "        Tr_velo_to_cam = np.array(obj, dtype=np.float32)\n",
    "\n",
    "        return {'P2': P2.reshape(3, 4),\n",
    "                'P3': P3.reshape(3, 4),\n",
    "                'R_rect': R0.reshape(3, 3),\n",
    "                'Tr_velo2cam': Tr_velo_to_cam.reshape(3, 4)}\n",
    "\n",
    "    def cart2hom(self, pts_3d):\n",
    "        \"\"\"\n",
    "        :param pts: (N, 3 or 2)\n",
    "        :return pts_hom: (N, 4 or 3)\n",
    "        \"\"\"\n",
    "        pts_hom = np.hstack((pts_3d, np.ones((pts_3d.shape[0], 1), dtype=np.float32)))\n",
    "        return pts_hom\n",
    "\n",
    "    # ===========================\n",
    "    # ------- 3d to 3d ----------\n",
    "    # ===========================\n",
    "    def project_velo_to_ref(self, pts_3d_velo):\n",
    "        pts_3d_velo = self.cart2hom(pts_3d_velo)  # nx4\n",
    "        return np.dot(pts_3d_velo, np.transpose(self.V2C))\n",
    "\n",
    "    def project_ref_to_velo(self, pts_3d_ref):\n",
    "        pts_3d_ref = self.cart2hom(pts_3d_ref)  # nx4\n",
    "        return np.dot(pts_3d_ref, np.transpose(self.C2V))\n",
    "\n",
    "    def project_rect_to_ref(self, pts_3d_rect):\n",
    "        ''' Input and Output are nx3 points '''\n",
    "        return np.transpose(np.dot(np.linalg.inv(self.R0), np.transpose(pts_3d_rect)))\n",
    "\n",
    "    def project_ref_to_rect(self, pts_3d_ref):\n",
    "        ''' Input and Output are nx3 points '''\n",
    "        return np.transpose(np.dot(self.R0, np.transpose(pts_3d_ref)))\n",
    "\n",
    "    def project_rect_to_velo(self, pts_3d_rect):\n",
    "        ''' Input: nx3 points in rect camera coord.\n",
    "            Output: nx3 points in velodyne coord.\n",
    "        '''\n",
    "        pts_3d_ref = self.project_rect_to_ref(pts_3d_rect)\n",
    "        return self.project_ref_to_velo(pts_3d_ref)\n",
    "    # 11111\n",
    "    def project_velo_to_rect(self, pts_3d_velo):\n",
    "        pts_3d_ref = self.project_velo_to_ref(pts_3d_velo)\n",
    "        return self.project_ref_to_rect(pts_3d_ref)\n",
    "\n",
    "    # ===========================\n",
    "    # ------- 3d to 2d ----------\n",
    "    # ===========================\n",
    "    # 22222\n",
    "    def project_rect_to_image(self, pts_3d_rect):\n",
    "        ''' Input: nx3 points in rect camera coord.\n",
    "            Output: nx2 points in image2 coord.\n",
    "        '''\n",
    "        pts_3d_rect = self.cart2hom(pts_3d_rect)\n",
    "        pts_2d = np.dot(pts_3d_rect, np.transpose(self.P))  # nx3\n",
    "        pts_2d[:, 0] /= pts_2d[:, 2]\n",
    "        pts_2d[:, 1] /= pts_2d[:, 2]\n",
    "        pts_2d_dis = dis = np.sqrt((pts_3d_rect[:, 0] ** 2) + (pts_3d_rect[:, 1] ** 2) + (pts_3d_rect[:, 2] ** 2))\n",
    "        return pts_2d[:, 0:2], pts_2d_dis\n",
    "\n",
    "    def project_velo_to_image(self, pts_3d_velo):\n",
    "        ''' Input: nx3 points in velodyne coord.\n",
    "            Output: nx2 points in image2 coord.\n",
    "        '''\n",
    "        pts_3d_rect = self.project_velo_to_rect(pts_3d_velo)\n",
    "        return self.project_rect_to_image(pts_3d_rect)\n",
    "\n",
    "    # ===========================\n",
    "    # ------- 2d to 3d ----------\n",
    "    # ===========================\n",
    "    def project_image_to_rect(self, uv_depth):\n",
    "        ''' Input: nx3 first two channels are uv, 3rd channel\n",
    "                   is depth in rect camera coord.\n",
    "            Output: nx3 points in rect camera coord.\n",
    "        '''\n",
    "        n = uv_depth.shape[0]\n",
    "        x = ((uv_depth[:, 0] - self.c_u) * uv_depth[:, 2]) / self.f_u + self.b_x\n",
    "        y = ((uv_depth[:, 1] - self.c_v) * uv_depth[:, 2]) / self.f_v + self.b_y\n",
    "        pts_3d_rect = np.zeros((n, 3))\n",
    "        pts_3d_rect[:, 0] = x\n",
    "        pts_3d_rect[:, 1] = y\n",
    "        pts_3d_rect[:, 2] = uv_depth[:, 2]\n",
    "        return pts_3d_rect\n",
    "\n",
    "    def project_image_to_velo(self, uv_depth):\n",
    "        pts_3d_rect = self.project_image_to_rect(uv_depth)\n",
    "        return self.project_rect_to_velo(pts_3d_rect)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "ca=Calibration(\"./kitty/calib/000001.txt\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 流程模拟"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "原始点云坐标pt\n",
      "[[5. 0. 0. 1.]\n",
      " [5. 0. 1. 1.]]\n",
      " \n",
      "ca.V2C.shape (3, 4)\n",
      "velo-ref\n",
      "[[ 0.03 -0.    4.72]\n",
      " [ 0.03 -1.    4.74]]\n",
      " \n",
      "ref-rect:\n",
      "[[-0.   -0.    4.72]\n",
      " [-0.   -1.    4.73]]\n",
      " \n",
      "内参后的uv坐标\n",
      "[[618.  172. ]\n",
      " [618.   20.3]]\n",
      " \n"
     ]
    }
   ],
   "source": [
    "velo=[[5,0,0],\n",
    "     [5,0,1]]\n",
    "velo=np.array(velo)\n",
    "def qmprint(name,pts):\n",
    "    print(name)\n",
    "    for i in range(len(pts)):\n",
    "        for j in range(len(pts[i])):\n",
    "            pts[i][j]=float(str(pts[i][j])[:4])\n",
    "    print(pts)\n",
    "    print(\" \")\n",
    "##################################velo-ref\n",
    "pts=ca.cart2hom(velo)\n",
    "qmprint(\"原始点云坐标pt\",pts)\n",
    "print(\"ca.V2C.shape\",ca.V2C.shape)\n",
    "pts=np.dot(pts, np.transpose(ca.V2C))\n",
    "qmprint(\"velo-ref\",pts)\n",
    "##################################ref-rect\n",
    "pts=np.transpose(np.dot(ca.R0, np.transpose(pts)))\n",
    "qmprint(\"ref-rect:\",pts)\n",
    "##################################rect-img\n",
    "pts = ca.cart2hom(pts)\n",
    "pts = np.dot(pts, np.transpose(ca.P))  # nx3\n",
    "pts[:, 0] /= pts[:, 2]\n",
    "pts[:, 1] /= pts[:, 2]\n",
    "qmprint(\"内参后的uv坐标\",pts[:, 0:2])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "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.7.9"
  },
  "toc-autonumbering": true
 },
 "nbformat": 4,
 "nbformat_minor": 4
}