Skip to content

Instantly share code, notes, and snippets.

@YoshiRi

YoshiRi/Homography Decomposition Test.ipynb

Last active June 2, 2018 17:57
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save YoshiRi/cdb901b784057b85afb4abd21da99f2f to your computer and use it in GitHub Desktop.
Save YoshiRi/cdb901b784057b85afb4abd21da99f2f to your computer and use it in GitHub Desktop.
Download ZIP
Homography decomposition test
Display the source blob
Display the rendered blob
Raw
Homography Decomposition Test.ipynb
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Init settings"
]
},
{
"cell_type": "code",
"execution_count": 23,
"metadata": {
"collapsed": true
},
"outputs": [],
"source": [
"import numpy as np\n",
"import cv2\n",
"import math"
]
},
{
"cell_type": "code",
"execution_count": 24,
"metadata": {
"collapsed": true
},
"outputs": [],
"source": [
"W = 0.3\n",
"L = 0.2\n",
"D = 0.3\n",
"\n",
"points = np.array([[W/2,L/2,D],[-W/2,L/2,D],[W/2,-L/2,D],[-W/2,-L/2,D]])"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Rotation matrix code is from [here](https://www.learnopencv.com/rotation-matrix-to-euler-angles/)"
]
},
{
"cell_type": "code",
"execution_count": 25,
"metadata": {
"collapsed": true
},
"outputs": [],
"source": [
"# rotation matrix\n",
"def rotM(p):\n",
" # 回転行列を計算する\n",
" px = p[0]\n",
" py = p[1]\n",
" pz = p[2]\n",
"\n",
" # 物体座標系の 1->2->3 軸で回転させる\n",
" Rx = np.array([[1, 0, 0],\n",
" [0, np.cos(px), np.sin(px)],\n",
" [0, -np.sin(px), np.cos(px)]])\n",
" Ry = np.array([[np.cos(py), 0, -np.sin(py)],\n",
" [0, 1, 0],\n",
" [np.sin(py), 0, np.cos(py)]])\n",
" Rz = np.array([[np.cos(pz), np.sin(pz), 0],\n",
" [-np.sin(pz), np.cos(pz), 0],\n",
" [0, 0, 1]])\n",
" R = Rz.dot(Ry).dot(Rx)\n",
" return R\n",
"\n",
"# Calculates Rotation Matrix given euler angles.\n",
"def eulerAnglesToRotationMatrix(theta) :\n",
" \n",
" R_x = np.array([[1, 0, 0 ],\n",
" [0, math.cos(theta[0]), -math.sin(theta[0]) ],\n",
" [0, math.sin(theta[0]), math.cos(theta[0]) ]\n",
" ])\n",
" \n",
" \n",
" \n",
" R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1]) ],\n",
" [0, 1, 0 ],\n",
" [-math.sin(theta[1]), 0, math.cos(theta[1]) ]\n",
" ])\n",
" \n",
" R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0],\n",
" [math.sin(theta[2]), math.cos(theta[2]), 0],\n",
" [0, 0, 1]\n",
" ])\n",
" \n",
" \n",
" R = np.dot(R_z, np.dot( R_y, R_x ))\n",
" \n",
" return R\n",
"\n",
"# Checks if a matrix is a valid rotation matrix.\n",
"def isRotationMatrix(R) :\n",
" Rt = np.transpose(R)\n",
" shouldBeIdentity = np.dot(Rt, R)\n",
" I = np.identity(3, dtype = R.dtype)\n",
" n = np.linalg.norm(I - shouldBeIdentity)\n",
" return n < 1e-6\n",
" \n",
" \n",
"# Calculates rotation matrix to euler angles\n",
"# The result is the same as MATLAB except the order\n",
"# of the euler angles ( x and z are swapped ).\n",
"def rotationMatrixToEulerAngles(R) :\n",
" \n",
" assert(isRotationMatrix(R))\n",
" \n",
" sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0])\n",
" \n",
" singular = sy < 1e-6\n",
" \n",
" if not singular :\n",
" x = math.atan2(R[2,1] , R[2,2])\n",
" y = math.atan2(-R[2,0], sy)\n",
" z = math.atan2(R[1,0], R[0,0])\n",
" else :\n",
" x = math.atan2(-R[1,2], R[1,1])\n",
" y = math.atan2(-R[2,0], sy)\n",
" z = 0\n",
" \n",
" return np.array([x, y, z])"
]
},
{
"cell_type": "code",
"execution_count": 26,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"[[ 0.8660254 0. 0.5 ]\n",
" [ 0. 1. 0. ]\n",
" [-0.5 0. 0.8660254]]\n",
"[[ 0.1]\n",
" [ 0. ]\n",
" [ 0. ]]\n"
]
}
],
"source": [
"theta = [0, 30/180*math.pi, 0/180*math.pi]\n",
"t = np.array([0.1,0,0]).reshape(3,1)\n",
"R = eulerAnglesToRotationMatrix(theta)\n",
"print(R)\n",
"print(t)\n",
"Proj = np.concatenate([R, t], axis=1)"
]
},
{
"cell_type": "code",
"execution_count": 27,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"[[ 0.15 -0.15 0.15 -0.15]\n",
" [ 0.1 0.1 -0.1 -0.1 ]\n",
" [ 0.3 0.3 0.3 0.3 ]]\n",
"[[ 0.37990381 0.12009619 0.37990381 0.12009619]\n",
" [ 0.1 0.1 -0.1 -0.1 ]\n",
" [ 0.18480762 0.33480762 0.18480762 0.33480762]]\n"
]
}
],
"source": [
"print(points.T)\n",
"points2 = R.dot(points.T) + t\n",
"print(points2)"
]
},
{
"cell_type": "code",
"execution_count": 28,
"metadata": {
"collapsed": true
},
"outputs": [],
"source": [
"def Projection(points):\n",
" h,w = points.shape\n",
" if h != 3:\n",
" print(\"Input do not match our style!\")\n",
" Dep = points[2,:]\n",
" Depth = np.tile(Dep,(2,1))\n",
" return points[0:2,:]/Depth"
]
},
{
"cell_type": "code",
"execution_count": 29,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"[[ 0.5 -0.5 0.5 -0.5 ]\n",
" [ 0.33333333 0.33333333 -0.33333333 -0.33333333]]\n",
"[[ 2.05567177 0.35870208 2.05567177 0.35870208]\n",
" [ 0.54110323 0.29867898 -0.54110323 -0.29867898]]\n",
"[[[ 0.5 -0.5 ]]\n",
"\n",
" [[ 0.5 -0.5 ]]\n",
"\n",
" [[ 0.33333333 0.33333333]]\n",
"\n",
" [[-0.33333333 -0.33333333]]]\n"
]
}
],
"source": [
"ip = Projection(points.T)\n",
"ip2 = Projection(points2)\n",
"print(ip)\n",
"print(ip2)\n",
"print(ip.reshape(-1,1,2))"
]
},
{
"cell_type": "code",
"execution_count": 30,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"[[ 0.99999996 0. 0.96225041]\n",
" [ 0. 1.15470054 0. ]\n",
" [-0.57735027 0. 1. ]]\n"
]
}
],
"source": [
"#H, mask = cv2.findHomography(ip.reshape(-1,1,2), ip2.reshape(-1,1,2), cv2.RANSAC,5.0)\n",
"H, mask = cv2.findHomography(ip.T, ip2.T, cv2.RANSAC,5.0)\n",
"print(H)"
]
},
{
"cell_type": "code",
"execution_count": 31,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"(4, [array([[ 0.7049044 , 0. , 0.70930232],\n",
" [ 0. , 1. , 0. ],\n",
" [-0.70930232, 0. , 0.7049044 ]]),\n",
" array([[ 0.7049044 , 0. , 0.70930232],\n",
" [ 0. , 1. , 0. ],\n",
" [-0.70930232, 0. , 0.7049044 ]]),\n",
" array([[ 0.86602541, 0. , 0.5 ],\n",
" [ 0. , 1. , 0. ],\n",
" [-0.5 , 0. , 0.86602541]]),\n",
" array([[ 0.86602541, 0. , 0.5 ],\n",
" [ 0. , 1. , 0. ],\n",
" [-0.5 , 0. , 0.86602541]])], [array([[ 0.20333138],\n",
" [ 0. ],\n",
" [ 0.26413527]]), array([[-0.20333138],\n",
" [-0. ],\n",
" [-0.26413527]]), array([[ 3.33333301e-01],\n",
" [ 0.00000000e+00],\n",
" [ -6.39600919e-09]]), array([[ -3.33333301e-01],\n",
" [ -0.00000000e+00],\n",
" [ 6.39600919e-09]])], [array([[ 0.79240583],\n",
" [ 0. ],\n",
" [ 0.60999426]]), array([[-0.79240583],\n",
" [-0. ],\n",
" [-0.60999426]]), array([[ -1.17363704e-07],\n",
" [ 0.00000000e+00],\n",
" [ 1.00000000e+00]]), array([[ 1.17363704e-07],\n",
" [ -0.00000000e+00],\n",
" [ -1.00000000e+00]])])"
]
},
"execution_count": 31,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"retval, rotations, translations, normals = cv2.decomposeHomographyMat(H,np.eye(3))\n",
"cv2.decomposeHomographyMat(H,np.eye(3))"
]
},
{
"cell_type": "code",
"execution_count": 32,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"translation\n",
"[[ 0.20333138]\n",
" [ 0. ]\n",
" [ 0.26413527]]\n",
"[[-0.20333138]\n",
" [-0. ]\n",
" [-0.26413527]]\n",
"[[ 3.33333301e-01]\n",
" [ 0.00000000e+00]\n",
" [ -6.39600919e-09]]\n",
"[[ -3.33333301e-01]\n",
" [ -0.00000000e+00]\n",
" [ 6.39600919e-09]]\n",
"normals\n",
"[[ 0.79240583]\n",
" [ 0. ]\n",
" [ 0.60999426]]\n",
"[[-0.79240583]\n",
" [-0. ]\n",
" [-0.60999426]]\n",
"[[ -1.17363704e-07]\n",
" [ 0.00000000e+00]\n",
" [ 1.00000000e+00]]\n",
"[[ 1.17363704e-07]\n",
" [ -0.00000000e+00]\n",
" [ -1.00000000e+00]]\n"
]
}
],
"source": [
"print(\"translation\")\n",
"for t in translations:\n",
" print(t)\n",
"\n",
"print(\"normals\")\n",
"for n in normals:\n",
" print(n)\n",
" "
]
},
{
"cell_type": "code",
"execution_count": 33,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"[ 0. 45.17817882 0. ]\n",
"[ 0. 45.17817882 0. ]\n",
"[ 0. 29.99999972 0. ]\n",
"[ 0. 29.99999972 0. ]\n"
]
}
],
"source": [
"for R in rotations:\n",
" print(rotationMatrixToEulerAngles(R)*180/math.pi)"
]
},
{
"cell_type": "code",
"execution_count": 36,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"[[ 0.9912407 ]\n",
" [ 0. ]\n",
" [-0.13206766]]\n",
"[[-0.9912407 ]\n",
" [ 0. ]\n",
" [ 0.13206766]]\n",
"[[ 0.49999989]\n",
" [ 0. ]\n",
" [ 0.86602546]]\n",
"[[-0.49999989]\n",
" [ 0. ]\n",
" [-0.86602546]]\n"
]
}
],
"source": [
"# positive depth constraint?\n",
"for R,n in zip(rotations,normals):\n",
" print(R.dot(n))"
]
},
{
"cell_type": "code",
"execution_count": 38,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"[[ 0.60999426]]\n",
"[[-0.60999426]]\n",
"[[ 1.]]\n",
"[[-1.]]\n"
]
}
],
"source": [
"n_ref = np.float32([0,0,1]).reshape(3,1)\n",
"for n in normals:\n",
" print(np.dot(n.T,n_ref))"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"collapsed": true
},
"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.5.3"
}
},
"nbformat": 4,
"nbformat_minor": 2
}
Display the source blob
Display the rendered blob
Raw
Practice.ipynb
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment