diff --git a/doc/source/tutorials/04-optic-flow.ipynb b/doc/source/tutorials/04-optic-flow.ipynb
index 748a28861d7b11c5ba4635b525cfb761db4315d5..f79e1f561e4c2368b879e242d7acff990a3db877 100644
--- a/doc/source/tutorials/04-optic-flow.ipynb
+++ b/doc/source/tutorials/04-optic-flow.ipynb
@@ -657,23 +657,210 @@
},
{
"cell_type": "code",
- "execution_count": null,
+ "execution_count": 103,
"metadata": {},
- "outputs": [],
+ "outputs": [
+ {
+ "data": {
+ "text/html": [
+ "<div>\n",
+ "<style scoped>\n",
+ " .dataframe tbody tr th:only-of-type {\n",
+ " vertical-align: middle;\n",
+ " }\n",
+ "\n",
+ " .dataframe tbody tr th {\n",
+ " vertical-align: top;\n",
+ " }\n",
+ "\n",
+ " .dataframe thead tr th {\n",
+ " text-align: left;\n",
+ " }\n",
+ "\n",
+ " .dataframe thead tr:last-of-type th {\n",
+ " text-align: right;\n",
+ " }\n",
+ "</style>\n",
+ "<table border=\"1\" class=\"dataframe\">\n",
+ " <thead>\n",
+ " <tr>\n",
+ " <th></th>\n",
+ " <th colspan=\"3\" halign=\"left\">location</th>\n",
+ " <th colspan=\"3\" halign=\"left\">zyx</th>\n",
+ " <th colspan=\"3\" halign=\"left\">location</th>\n",
+ " <th colspan=\"3\" halign=\"left\">zyx</th>\n",
+ " </tr>\n",
+ " <tr>\n",
+ " <th></th>\n",
+ " <th>x</th>\n",
+ " <th>y</th>\n",
+ " <th>z</th>\n",
+ " <th>alpha_0</th>\n",
+ " <th>alpha_1</th>\n",
+ " <th>alpha_2</th>\n",
+ " <th>dx</th>\n",
+ " <th>dy</th>\n",
+ " <th>dz</th>\n",
+ " <th>dalpha_0</th>\n",
+ " <th>dalpha_1</th>\n",
+ " <th>dalpha_2</th>\n",
+ " </tr>\n",
+ " <tr>\n",
+ " <th>id</th>\n",
+ " <th></th>\n",
+ " <th></th>\n",
+ " <th></th>\n",
+ " <th></th>\n",
+ " <th></th>\n",
+ " <th></th>\n",
+ " <th></th>\n",
+ " <th></th>\n",
+ " <th></th>\n",
+ " <th></th>\n",
+ " <th></th>\n",
+ " <th></th>\n",
+ " </tr>\n",
+ " </thead>\n",
+ " <tbody>\n",
+ " <tr>\n",
+ " <th>2</th>\n",
+ " <td>-0.25</td>\n",
+ " <td>-0.50</td>\n",
+ " <td>3.0</td>\n",
+ " <td>0.0</td>\n",
+ " <td>0.0</td>\n",
+ " <td>0.0</td>\n",
+ " <td>0.25</td>\n",
+ " <td>0.00</td>\n",
+ " <td>0.0</td>\n",
+ " <td>0.0</td>\n",
+ " <td>0.0</td>\n",
+ " <td>0.0</td>\n",
+ " </tr>\n",
+ " <tr>\n",
+ " <th>3</th>\n",
+ " <td>0.00</td>\n",
+ " <td>-0.50</td>\n",
+ " <td>3.0</td>\n",
+ " <td>0.0</td>\n",
+ " <td>0.0</td>\n",
+ " <td>0.0</td>\n",
+ " <td>0.25</td>\n",
+ " <td>0.00</td>\n",
+ " <td>0.0</td>\n",
+ " <td>0.0</td>\n",
+ " <td>0.0</td>\n",
+ " <td>0.0</td>\n",
+ " </tr>\n",
+ " <tr>\n",
+ " <th>4</th>\n",
+ " <td>0.25</td>\n",
+ " <td>-0.50</td>\n",
+ " <td>3.0</td>\n",
+ " <td>0.0</td>\n",
+ " <td>0.0</td>\n",
+ " <td>0.0</td>\n",
+ " <td>0.25</td>\n",
+ " <td>0.00</td>\n",
+ " <td>0.0</td>\n",
+ " <td>0.0</td>\n",
+ " <td>0.0</td>\n",
+ " <td>0.0</td>\n",
+ " </tr>\n",
+ " <tr>\n",
+ " <th>5</th>\n",
+ " <td>0.50</td>\n",
+ " <td>-0.50</td>\n",
+ " <td>3.0</td>\n",
+ " <td>0.0</td>\n",
+ " <td>0.0</td>\n",
+ " <td>0.0</td>\n",
+ " <td>0.25</td>\n",
+ " <td>0.00</td>\n",
+ " <td>0.0</td>\n",
+ " <td>0.0</td>\n",
+ " <td>0.0</td>\n",
+ " <td>0.0</td>\n",
+ " </tr>\n",
+ " <tr>\n",
+ " <th>6</th>\n",
+ " <td>-0.50</td>\n",
+ " <td>-0.25</td>\n",
+ " <td>3.0</td>\n",
+ " <td>0.0</td>\n",
+ " <td>0.0</td>\n",
+ " <td>0.0</td>\n",
+ " <td>-1.00</td>\n",
+ " <td>0.25</td>\n",
+ " <td>0.0</td>\n",
+ " <td>0.0</td>\n",
+ " <td>0.0</td>\n",
+ " <td>0.0</td>\n",
+ " </tr>\n",
+ " </tbody>\n",
+ "</table>\n",
+ "</div>"
+ ],
+ "text/plain": [
+ " location zyx location zyx \\\n",
+ " x y z alpha_0 alpha_1 alpha_2 dx dy dz dalpha_0 \n",
+ "id \n",
+ "2 -0.25 -0.50 3.0 0.0 0.0 0.0 0.25 0.00 0.0 0.0 \n",
+ "3 0.00 -0.50 3.0 0.0 0.0 0.0 0.25 0.00 0.0 0.0 \n",
+ "4 0.25 -0.50 3.0 0.0 0.0 0.0 0.25 0.00 0.0 0.0 \n",
+ "5 0.50 -0.50 3.0 0.0 0.0 0.0 0.25 0.00 0.0 0.0 \n",
+ "6 -0.50 -0.25 3.0 0.0 0.0 0.0 -1.00 0.25 0.0 0.0 \n",
+ "\n",
+ " \n",
+ " dalpha_1 dalpha_2 \n",
+ "id \n",
+ "2 0.0 0.0 \n",
+ "3 0.0 0.0 \n",
+ "4 0.0 0.0 \n",
+ "5 0.0 0.0 \n",
+ "6 0.0 0.0 "
+ ]
+ },
+ "execution_count": 103,
+ "metadata": {},
+ "output_type": "execute_result"
+ }
+ ],
"source": [
"import pkg_resources\n",
"from navipy.database import DataBase\n",
+ "import time\n",
"mydbfile = pkg_resources.resource_filename(\n",
" 'navipy', 'resources/database.db')\n",
"mydb = DataBase(mydbfile)\n",
"mytraj = mydb.posorients\n",
"mytrajdiff = mytraj.differentiate(periods=1)\n",
"mytrajvel = mytraj.join(mytrajdiff)\n",
- "mytrajvel.dropna().head()\n",
- "\n",
- "my_opticflow=pd.DataFrame(index=mytrajvel.index,\n",
- " columns=pd.MultiIndex.from_product([points_name,['rof','hof','vof']]))\n",
- "my_opticflow=my_opticflow.swaplevel(axis=1)\n",
+ "mytrajvel.dropna().head()"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "### without multiprocessing"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 104,
+ "metadata": {},
+ "outputs": [
+ {
+ "name": "stdout",
+ "output_type": "stream",
+ "text": [
+ "Optic flow was calculated on 24 frame in 421.6605281829834s\n"
+ ]
+ }
+ ],
+ "source": [
+ "t_start = time.time()\n",
"horizontal_of = np.zeros((mytrajvel.shape[0],*mydb.viewing_directions[...,0].shape))\n",
"vertical_of = horizontal_of.copy()\n",
"\n",
@@ -684,7 +871,71 @@
" _,hof,vof = optic_flow(distance,mydb.viewing_directions,velocity)\n",
" # save the results in df\n",
" horizontal_of[ii,...]=hof\n",
- " vertical_of[ii,...]=vof"
+ " vertical_of[ii,...]=vof\n",
+ " \n",
+ "t_elapse = time.time()-t_start\n",
+ "\n",
+ "print('Optic flow was calculated on {} frame in {}s'.format(mytrajvel.dropna().shape[0],\n",
+ " t_elapse))"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "### with mulitprocessing"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 105,
+ "metadata": {},
+ "outputs": [
+ {
+ "name": "stdout",
+ "output_type": "stream",
+ "text": [
+ "Optic flow was calculated on 24 frame in 104.01745867729187s\n"
+ ]
+ }
+ ],
+ "source": [
+ "from multiprocessing import Pool\n",
+ "from functools import partial\n",
+ "\n",
+ "def calc_optic_flow(ii,dbfile,trajvel):\n",
+ " db = DataBase(dbfile)\n",
+ " velocity = trajvel.iloc[ii,:]\n",
+ " scene = db.scene(velocity)\n",
+ " distance = scene[...,3,0]\n",
+ " # Calculate the optic flow of these points\n",
+ " _,hof,vof = optic_flow(distance,db.viewing_directions,velocity)\n",
+ " return (ii, hof, vof)\n",
+ "\n",
+ "t_start = time.time()\n",
+ "\n",
+ "indeces = list(np.arange(mytrajvel.dropna().shape[0]))\n",
+ "with Pool() as p:\n",
+ " result = p.map(\n",
+ " partial(calc_optic_flow,\n",
+ " dbfile=mydbfile,\n",
+ " trajvel=mytrajvel.dropna()),\n",
+ " indeces)\n",
+ "# Place results in a usable array\n",
+ "nb_of = len(result)\n",
+ "size_of = result[0][1].shape\n",
+ "\n",
+ "horizontal_of = np.zeros((nb_of,*size_of))\n",
+ "vertical_of = horizontal_of.copy()\n",
+ "for flow in result:\n",
+ " ii = flow[0]\n",
+ " horizontal_of[ii,...]=flow[1]\n",
+ " vertical_of[ii,...]=flow[2]\n",
+ " \n",
+ "t_elapse = time.time()-t_start\n",
+ "\n",
+ "print('Optic flow was calculated on {} frame in {}s'.format(mytrajvel.dropna().shape[0],\n",
+ " t_elapse))"
]
},
{
diff --git a/navipy/maths/quaternion.py b/navipy/maths/quaternion.py
index 51ef70fef44ce5c42c6f24bd30cf46224c6b0f39..3df5b277adb90bf35c04c09c8694dd80ee349a61 100644
--- a/navipy/maths/quaternion.py
+++ b/navipy/maths/quaternion.py
@@ -109,7 +109,7 @@ def from_matrix(matrix):
"""
r = matrix
# Split cases according to equation 145
- if r[1, 1] > -r[2, 2] or r[0, 0] > -r[1, 1] or r[0, 0] > -r[2, 2]:
+ if (r[1, 1] >= -r[2, 2]) and (r[0, 0] >= -r[1, 1]) and (r[0, 0] >= -r[2, 2]):
# equation 141
return (1/2)*np.array([np.sqrt(1 + r[0, 0] + r[1, 1] + r[2, 2]),
(r[1, 2] - r[2, 1]) /
@@ -119,7 +119,7 @@ def from_matrix(matrix):
(r[0, 1] - r[1, 0]) /
np.sqrt(1 + r[0, 0] + r[1, 1] + r[2, 2])])
- if r[1, 1] < -r[2, 2] or r[0, 0] > r[1, 1] or r[0, 0] > r[2, 2]:
+ if (r[1, 1] < -r[2, 2]) and (r[0, 0] > r[1, 1]) and (r[0, 0] > r[2, 2]):
# equation 142
return (1/2)*np.array([(r[1, 2] - r[2, 1]) /
np.sqrt(1 + r[0, 0] - r[1, 1] - r[2, 2]),
@@ -128,7 +128,7 @@ def from_matrix(matrix):
np.sqrt(1 + r[0, 0] - r[1, 1] - r[2, 2]),
(r[2, 0] + r[0, 2]) /
np.sqrt(1 + r[0, 0] - r[1, 1] - r[2, 2])])
- if r[1, 1] > r[2, 2] or r[0, 0] < r[1, 1] or r[0, 0] < -r[2, 2]:
+ if (r[1, 1] > r[2, 2]) and (r[0, 0] < r[1, 1]) and (r[0, 0] < -r[2, 2]):
# equation 143
return (1/2)*np.array([(r[2, 0] - r[0, 2]) /
np.sqrt(1 - r[0, 0] + r[1, 1] - r[2, 2]),
@@ -137,7 +137,7 @@ def from_matrix(matrix):
np.sqrt(1 - r[0, 0] + r[1, 1] - r[2, 2]),
(r[1, 2] + r[2, 1]) /
np.sqrt(1 - r[0, 0] + r[1, 1] - r[2, 2])])
- if r[1, 1] < r[2, 2] or r[0, 0] < -r[1, 1] or r[0, 0] < r[2, 2]:
+ if (r[1, 1] < r[2, 2]) and (r[0, 0] < -r[1, 1]) and (r[0, 0] < r[2, 2]):
# equation 144
return (1/2)*np.array([(r[0, 1] - r[1, 0]) /
np.sqrt(1 - r[0, 0] - r[1, 1] + r[2, 2]),
diff --git a/navipy/sensors/renderer.py b/navipy/sensors/renderer.py
index 70982614ad52a8d798d1c20c511306d2a7f3f8f7..befcae63177c1b38e4255a1c615259e2629d20c5 100644
--- a/navipy/sensors/renderer.py
+++ b/navipy/sensors/renderer.py
@@ -600,7 +600,7 @@ class BlenderRender(AbstractRender):
translate=posorient.loc['location'],
axes=convention)
if self._renderaxis == '+x':
- initrot = quatmatrix([0.5, 0.5, -0.5, -0.5])
+ initrot = quatmatrix([0.5, -0.5, 0.5, 0.5])
# The camera is aligned in -z
# A rotation along z wll thus roll the camera
# Althougth the camera should yaw in such case