Skip to content
Snippets Groups Projects
Commit 0a5087d5 authored by Olivier Bertrand's avatar Olivier Bertrand
Browse files

Start addinng dlt with more than 11 params

parent 182b80f0
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
doc/source/tutorials/02-recording-animal-trajectory.ipynb
+ 109
− 7
View file @ 0a5087d5
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
# Recording animal trajectory # Recording animal trajectory
## Camera calibration ## Camera recording
A camera is an optical intrument for aquiring images. Cameras are composed of a sensors (converting photon to electrical signal) and a lens (foccussing light rays on the sensors). \ A camera is an optical intrument for aquiring images. Cameras are composed of a sensors (converting photon to electrical signal) and a lens (foccussing light rays on the sensors).
Many experimental paradigm require the recording of animal motion with one or more camera. \ Many experimental paradigm require the recording of animal motion with one or more camera.
The experimenter is more interested by the position of the animal in his or her arena (world \ The experimenter is more interested by the position of the animal in his or her arena (world
coordinate system) than by the position of the animal within the image (camera-coordinate system). Therefore, we need to transform the position of the animal in the image to its position in the world. To do so, we need to calibrate our camera. This calibration require two steps: coordinate system) than by the position of the animal within the image (camera-coordinate system). Therefore, we need to transform the position of the animal in the image to its position in the world. To do so, we need to calibrate our camera. This calibration require two steps:
* remove distortion from the lens (intrinsic calibration) * remove distortion from the lens (intrinsic calibration)
* determine the position and orientation (i.e. pose) of the camera in the environment (extrinsic calibration) * determine the position and orientation (i.e. pose) of the camera in the environment (extrinsic calibration)
### Intrinsic calibration ### Direct Linear Transformation (DLT)
When we observe an animal through a camera, we only observe the projection of the animal. For example the center of mass of the animal in three dimension is projected in two dimension on the camera, i.e. in the camera plane, and only these two dimensions are visible from the camera. Mathematically, the point in 3D space is transformed into the camera space. This transformation can be described as follow:
$$
\begin{align}
\begin{bmatrix} u-u_0 \\ v-v_0 \\ -d \end{bmatrix} &= T \begin{bmatrix} x-x_0 \\ y-y_0 \\ z-z_0 \end{bmatrix} \\
&=c \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix}\begin{bmatrix} x-x_0 \\ y-y_0 \\ z-z_0 \end{bmatrix}
\end{align}
$$
with:
* $u$, $v$ the position of the animal on the camera
* $x$, $y$, $z$ the position of the animal in the environment
* $u_0$, $v_0$, the center of the camera
* $x_0$, $y_0$, $z_0$ the center of the environment (origin)
* $d$, the distance between the point on the camera and the point in the environment
* $c$, a constant of colinarity
**Note** The unit of $u$, $v$ , $u_0$, and $v_0$ have the same units than $x,y,z$. However, $u$ and $v$ will be usually measured in pixels, and thus we need to further transform $u$ and $v$ by a constant of proportionality. $u-u_0 \Rightarrow \lambda_u(u-u_0)$, and $v-v_0 \Rightarrow \lambda_v(v-v_0)$.
#### Calibration
We ultimatly want to have the position of the animal within its environment from two or more cameras. To be able to reconstruct (triangulate) the animal position, we first need to have a method to describe the transformation from camera to world and vice versa, i.e. obtained the parameters of the transformation from known points. This process is called camera calibration.
We need to express $u$ and $v$ as a function of $x$, $y$, and $z$.
$$
u = \frac{L_1x+L_2y+L_3z+L_4}{L_9x+L_{10}y+L_{11}z+L_1}\text{, and }
v = \frac{L_5x+L_6y+L_7z+L_8}{L_9x+L_{10}y+L_{11}z+L_1}
$$
the coefficients $L_1$ to $L_{11}$ are the DLT parameters that reflect the relationships between the environment frame of our animal and the image frame.
The system of equation above can rewritten as a product of matrices:
$$
\begin{bmatrix} u \\ v \end{bmatrix} = \begin{bmatrix} x & y & z & 1 & 0 & 0 & 0 & 0 & -ux & -uy & -uz \\ 0 & 0 & 0 & 0 & x & y & z & 1 & -vx & -vy & -vz \end{bmatrix}\begin{bmatrix} L_1 \\ L_2 \\ . \\ L_{10} \\ L_{11} \end{bmatrix}
$$
We can expand this equations for $n$ points visible on the camera:
$$
\begin{bmatrix}
u_1 \\ v_1 \\
u_2 \\ v_2 \\
... \\
u_{n-1} \\ v_{n-1} \\
u_n \\ v_n \\
\end{bmatrix} =
\begin{bmatrix}
x_1 & y_1 & z_1 & 1 & 0 & 0 & 0 & 0 & -u_1x_1 & -u_1y_1 & -u_1z_1 \\
0 & 0 & 0 & 0 & x_1 & y_1 & z_1 & 1 & -v_1x_1 & -v_1y_1 & -v_1z_1 \\
x_2 & y_2 & z_2 & 1 & 0 & 0 & 0 & 0 & -u_2x_2 & -u_2y_2 & -u_2z_2 \\
0 & 0 & 0 & 0 & x_2 & y_2 & z_2 & 1 & -v_2x_2 & -v_2y_2 & -v_2z_2 \\
. \\
x_{n-1} & y_{n-1} & z_{n-1} & 1 & 0 & 0 & 0 & 0 & -u_{n-1}x_{n-1} & -u_{n-1}y_{n-1} & -u_{n-1}z_{n-1} \\
0 & 0 & 0 & 0 & x_{n-1} & y_{n-1} & z_{n-1} & 1 & -v_{n-1}x_{n-1} & -v_{n-1}y_{n-1} & -v_{n-1}z_{n-1} \\
x_n & y_n & z_n & 1 & 0 & 0 & 0 & 0 & -u_nx_n & -u_ny_n & -u_nz_n \\
0 & 0 & 0 & 0 & x_n & y_n & z_n & 1 & -v_nx_n & -v_ny_n & -v_nz_n \end{bmatrix}
\begin{bmatrix} L_1 \\ L_2 \\ . \\ L_{10} \\ L_{11} \end{bmatrix}
$$
If we have 11 equations or more we can could deterime the DLT parameters from the system of equations. Since each points in the environment will lead to two variables $u$ and $v$, we only need 6 points to be able to obtain the DLT parameters
#### Reconstruction
#### Exercises
* Derive from the $r_11$ to $r_33$, $d$ and $c$ what are $L_1$ to $L_{11}$
#### 16 DLT parameters
The DLT method exposed above does not correct for optical distortion nor de-centering distortion.
$$
\frac{1}{R}\begin{bmatrix} u \\ v \end{bmatrix} =
\frac{1}{R}\begin{bmatrix}
x & y & z & 1 & 0 & 0 & 0 & 0 & -ux & -uy & -uz & \zeta r^2R & \zeta r^4R & \zeta r^6R & (r^2+2\zeta^2)R & \zeta\eta R \\
0 & 0 & 0 & 0 & x & y & z & 1 & -vx & -vy & -vz & \eta r^2R & \eta r^4R & \eta r^6R & \zeta\eta R & (r^2+2\eta^2)R\\ \end{bmatrix}\begin{bmatrix} L_1 \\ L_2 \\ . \\ L_{15} \\ L_{16} \end{bmatrix}
$$
with
* $R=L_9x + L_{10}y + L_{11}z +1$,
* $r^2 = \eta^2 + \zeta^2$,
* $\zeta = u-u_0$, and
* $\eta = v-v_0$
%% Cell type:markdown id: tags:
### Alternative method
A camera is an optical intrument for aquiring images. Cameras are composed of a sensors (converting photon to electrical signal) and a lens (foccussing light rays on the sensors).
Many experimental paradigm require the recording of animal motion with one or more camera.
The experimenter is more interested by the position of the animal in his or her arena (world
coordinate system) than by the position of the animal within the image (camera-coordinate system). Therefore, we need to transform the position of the animal in the image to its position in the world. To do so, we need to calibrate our camera. This calibration require two steps:
* remove distortion from the lens (intrinsic calibration)
* determine the position and orientation (i.e. pose) of the camera in the environment (extrinsic calibration)
#### Intrinsic calibration
`With Matlab <https://de.mathworks.com/help/vision/ug/single-camera-calibrator-app.html>`_ `With Matlab <https://de.mathworks.com/help/vision/ug/single-camera-calibrator-app.html>`_
`With Opencv <https://docs.opencv.org/3.3.1/dc/dbb/tutorial_py_calibration.html>`_ `With Opencv <https://docs.opencv.org/3.3.1/dc/dbb/tutorial_py_calibration.html>`_
### Extrinsic calibration #### Extrinsic calibration
To obtain the pose of the camera (i.e. the extrinsic parameters), we need To obtain the pose of the camera (i.e. the extrinsic parameters), we need
to find a transformation such that the projection of physical points matche to find a transformation such that the projection of physical points matche
the points position in 3D. In other words, a ray emanating from a projected the points position in 3D. In other words, a ray emanating from a projected
points should cross the physical points in 3D. points should cross the physical points in 3D.
The estimation of the camera pose is done by solving the PnP with Ransac. The estimation of the camera pose is done by solving the PnP with Ransac.
Using Ransac decrease the bad effect that outliers may have on the pose Using Ransac decrease the bad effect that outliers may have on the pose
estimation. estimation.
#### Building your own manhattan ## Building your own manhattan
The manhattan should contains at least 7 towers, and not all towers should The manhattan should contains at least 7 towers, and not all towers should
share the same plane or line. For best results it is recommended that the share the same plane or line. For best results it is recommended that the
towers are visible across the entire recorded range on the image. Note, towers are visible across the entire recorded range on the image. Note,
however, that not all towers need to be visible (at least 7 though). however, that not all towers need to be visible (at least 7 though).
To create your own manhattan you have to measure all towers (with a precise To create your own manhattan you have to measure all towers (with a precise
rules, or let it build by a workshop). The software need to know the position rules, or let it build by a workshop). The software need to know the position
of each tower as well. For example you can create a DataFrame containing the of each tower as well. For example you can create a DataFrame containing the
tower number and its position, and save it in hdf file. tower number and its position, and save it in hdf file.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
import pandas as pd import pandas as pd
import numpy as np import numpy as np
import pkg_resources import pkg_resources
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D from mpl_toolkits.mplot3d import Axes3D
from navipy.arenatools.cam_calib import plot_manhattan as cplot from navipy.arenatools.cam_calib import plot_manhattan as cplot
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
%matplotlib inline %matplotlib inline
manhattan_filename = 'manhattan.hdf' manhattan_filename = 'manhattan.hdf'
manhattan_key = 'manhattan' manhattan_key = 'manhattan'
num_points = 27 # Number of points num_points = 27 # Number of points
manhattan_3d = pd.DataFrame(index=range(num_points), manhattan_3d = pd.DataFrame(index=range(num_points),
columns=['x', 'y', 'z']) columns=['x', 'y', 'z'])
# Then you manually fill the manhattan measurement # Then you manually fill the manhattan measurement
manhattan_3d.loc[0, :] = [-10, -10, 4.66] manhattan_3d.loc[0, :] = [-10, -10, 4.66]
manhattan_3d.loc[1, :] = [-10, -5, 4.39] manhattan_3d.loc[1, :] = [-10, -5, 4.39]
manhattan_3d.loc[2, :] = [-10, -0, 4.63] manhattan_3d.loc[2, :] = [-10, -0, 4.63]
manhattan_3d.loc[3, :] = [-10, +5, 4.38] manhattan_3d.loc[3, :] = [-10, +5, 4.38]
manhattan_3d.loc[4, :] = [-10, +10, 4.85] manhattan_3d.loc[4, :] = [-10, +10, 4.85]
manhattan_3d.loc[5, :] = [-8.09, -0.25, 10.13] manhattan_3d.loc[5, :] = [-8.09, -0.25, 10.13]
manhattan_3d.loc[6, :] = [-5, -10, 4.52] manhattan_3d.loc[6, :] = [-5, -10, 4.52]
manhattan_3d.loc[7, :] = [-5, -5, 4.57] manhattan_3d.loc[7, :] = [-5, -5, 4.57]
manhattan_3d.loc[8, :] = [-5, -0, 4.36] manhattan_3d.loc[8, :] = [-5, -0, 4.36]
manhattan_3d.loc[9, :] = [-5, +5, 4.45] manhattan_3d.loc[9, :] = [-5, +5, 4.45]
manhattan_3d.loc[10, :] = [-5, +10, 4.43] manhattan_3d.loc[10, :] = [-5, +10, 4.43]
manhattan_3d.loc[11, :] = [0, -10, 4.70] manhattan_3d.loc[11, :] = [0, -10, 4.70]
manhattan_3d.loc[12, :] = [0, -5, 4.57] manhattan_3d.loc[12, :] = [0, -5, 4.57]
manhattan_3d.loc[13, :] = [0, +5, 4.16] manhattan_3d.loc[13, :] = [0, +5, 4.16]
manhattan_3d.loc[14, :] = [0, +10, 4.43] manhattan_3d.loc[14, :] = [0, +10, 4.43]
manhattan_3d.loc[15, :] = [+5, -10, 4.70] manhattan_3d.loc[15, :] = [+5, -10, 4.70]
manhattan_3d.loc[16, :] = [+5, -5, 4.56] manhattan_3d.loc[16, :] = [+5, -5, 4.56]
manhattan_3d.loc[17, :] = [+5, -0, 4.27] manhattan_3d.loc[17, :] = [+5, -0, 4.27]
manhattan_3d.loc[18, :] = [+5, +5, 4.49] manhattan_3d.loc[18, :] = [+5, +5, 4.49]
manhattan_3d.loc[19, :] = [+5, +10, 4.50] manhattan_3d.loc[19, :] = [+5, +10, 4.50]
manhattan_3d.loc[20, :] = [+8.62, -8.38, 10.19] manhattan_3d.loc[20, :] = [+8.62, -8.38, 10.19]
manhattan_3d.loc[21, :] = [+8.55, +8.72, 10.09] manhattan_3d.loc[21, :] = [+8.55, +8.72, 10.09]
manhattan_3d.loc[22, :] = [+10, -10, 4.51] manhattan_3d.loc[22, :] = [+10, -10, 4.51]
manhattan_3d.loc[23, :] = [+10, -5, 4.33] manhattan_3d.loc[23, :] = [+10, -5, 4.33]
manhattan_3d.loc[24, :] = [+10, -0, 4.69] manhattan_3d.loc[24, :] = [+10, -0, 4.69]
manhattan_3d.loc[25, :] = [+10, +5, 4.69] manhattan_3d.loc[25, :] = [+10, +5, 4.69]
manhattan_3d.loc[26, :] = [+10, +10, 4.71] manhattan_3d.loc[26, :] = [+10, +10, 4.71]
manhattan_3d *= 10 # Because millimeters are nicer to use than centimeter manhattan_3d *= 10 # Because millimeters are nicer to use than centimeter
``` ```
%% Output %% Output
--------------------------------------------------------------------------- ---------------------------------------------------------------------------
ModuleNotFoundError Traceback (most recent call last) ModuleNotFoundError Traceback (most recent call last)
<ipython-input-1-c4435f428126> in <module>() <ipython-input-1-c4435f428126> in <module>()
----> 1 import pandas as pd ----> 1 import pandas as pd
2 import numpy as np 2 import numpy as np
3 import pkg_resources 3 import pkg_resources
4 import matplotlib.pyplot as plt 4 import matplotlib.pyplot as plt
5 from mpl_toolkits.mplot3d import Axes3D 5 from mpl_toolkits.mplot3d import Axes3D
ModuleNotFoundError: No module named 'pandas' ModuleNotFoundError: No module named 'pandas'
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
The manhattan can be saved as follow, The manhattan can be saved as follow,
```manhattan_3d.to_hdf(manhattan_filename, manhattan_key)``` ```manhattan_3d.to_hdf(manhattan_filename, manhattan_key)```
and plotted and plotted
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
manhattan_imfile = list() manhattan_imfile = list()
manhattan_imfile.append(pkg_resources.resource_filename( manhattan_imfile.append(pkg_resources.resource_filename(
'navipy', 'resources/sample_experiment/Doussot_2018a/20180117_cam_0.tif')) 'navipy', 'resources/sample_experiment/Doussot_2018a/20180117_cam_0.tif'))
manhattan_imfile.append(pkg_resources.resource_filename( manhattan_imfile.append(pkg_resources.resource_filename(
'navipy', 'resources/sample_experiment/Doussot_2018a/20180117_cam_2.tif')) 'navipy', 'resources/sample_experiment/Doussot_2018a/20180117_cam_2.tif'))
f, axarr = plt.subplots(1, 2, figsize=(15, 8)) f, axarr = plt.subplots(1, 2, figsize=(15, 8))
for i, ax in enumerate(axarr): for i, ax in enumerate(axarr):
image = plt.imread(manhattan_imfile[i]) image = plt.imread(manhattan_imfile[i])
# Show the manhattan image # Show the manhattan image
ax.imshow(image, cmap='gray') ax.imshow(image, cmap='gray')
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
* notes the pixel position of each tower * notes the pixel position of each tower
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
# a filelist for pixels coordinates of towers # a filelist for pixels coordinates of towers
filenames_manhattans = list() filenames_manhattans = list()
filenames_manhattans.append(pkg_resources.resource_filename( filenames_manhattans.append(pkg_resources.resource_filename(
'navipy', 'resources/sample_experiment/Doussot_2018a/manhattan_cam_0_xypts.csv')) 'navipy', 'resources/sample_experiment/Doussot_2018a/manhattan_cam_0_xypts.csv'))
filenames_manhattans.append(pkg_resources.resource_filename( filenames_manhattans.append(pkg_resources.resource_filename(
'navipy', 'resources/sample_experiment/Doussot_2018a/manhattan_cam_2_xypts.csv')) 'navipy', 'resources/sample_experiment/Doussot_2018a/manhattan_cam_2_xypts.csv'))
# And then plot # And then plot
markersize =2 markersize =2
f, axarr = plt.subplots(1, 2, figsize=(15, 8)) f, axarr = plt.subplots(1, 2, figsize=(15, 8))
for i, ax in enumerate(axarr): for i, ax in enumerate(axarr):
image = plt.imread(manhattan_imfile[i]) image = plt.imread(manhattan_imfile[i])
manhattan_2d = pd.read_csv(filenames_manhattans[i], manhattan_2d = pd.read_csv(filenames_manhattans[i],
names=['x', 'y'], names=['x', 'y'],
header=0) header=0)
# Show the manhattan image # Show the manhattan image
ax.imshow(image, cmap='gray') ax.imshow(image, cmap='gray')
toplotx = manhattan_2d.x toplotx = manhattan_2d.x
# Because inverted y axis in image # Because inverted y axis in image
toploty = image.shape[0]-manhattan_2d.y toploty = image.shape[0]-manhattan_2d.y
markersize = 10 markersize = 10
# Plot marker # Plot marker
ax.plot(toplotx, ax.plot(toplotx,
toploty, 'o', toploty, 'o',
markersize=markersize, markersize=markersize,
markerfacecolor="None", markerfacecolor="None",
markeredgecolor='red', markeredgecolor='red',
markeredgewidth=2) markeredgewidth=2)
# Plot marker label # Plot marker label
for mi, xy in enumerate(zip(toplotx, toploty)): for mi, xy in enumerate(zip(toplotx, toploty)):
if np.any(np.isnan(xy)): if np.any(np.isnan(xy)):
continue continue
ax.text(xy[0]+2*markersize, ax.text(xy[0]+2*markersize,
xy[1]+2*markersize, xy[1]+2*markersize,
'{}'.format(mi), color='r', '{}'.format(mi), color='r',
horizontalalignment='left') horizontalalignment='left')
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
* solve the PnPRansac * solve the PnPRansac
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
#### Intrinsic and Extrinsic together #### Intrinsic and Extrinsic together
Alternatively you can use Matlab: Alternatively you can use Matlab:
`With Matlab <https://de.mathworks.com/help/vision/ref/estimateworldcamerapose.html>` `With Matlab <https://de.mathworks.com/help/vision/ref/estimateworldcamerapose.html>`
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
## Triangulation ## Triangulation
In computer vision triangulation refers to the process of determining a point in 3D space given its projections onto two, or more, images. In order to solve this problem it is necessary to know the parameters of the camera projection function from 3D to 2D for the cameras involved, in the simplest case represented by the camera matrices. Triangulation is sometimes also referred to as reconstruction. In computer vision triangulation refers to the process of determining a point in 3D space given its projections onto two, or more, images. In order to solve this problem it is necessary to know the parameters of the camera projection function from 3D to 2D for the cameras involved, in the simplest case represented by the camera matrices. Triangulation is sometimes also referred to as reconstruction.
The triangulation problem is in theory trivial. Since each point in an image corresponds to a line in 3D space, all points on the line in 3D are projected to the point in the image. If a pair of corresponding points in two, or more images, can be found it must be the case that they are the projection of a common 3D point x. The set of lines generated by the image points must intersect at x (3D point). The triangulation problem is in theory trivial. Since each point in an image corresponds to a line in 3D space, all points on the line in 3D are projected to the point in the image. If a pair of corresponding points in two, or more images, can be found it must be the case that they are the projection of a common 3D point x. The set of lines generated by the image points must intersect at x (3D point).
Richard Hartley and Andrew Zisserman (2003). Multiple View Geometry in computer vision. Cambridge University Press Richard Hartley and Andrew Zisserman (2003). Multiple View Geometry in computer vision. Cambridge University Press
### Stereo triangulation ### Stereo triangulation
### Multi-view triangulation ### Multi-view triangulation
#### Pair-wise combination #### Pair-wise combination
Triangulating projected points from 2 or more camera can be done by doing pairwise triangulation, and then calculation of the median. Triangulating projected points from 2 or more camera can be done by doing pairwise triangulation, and then calculation of the median.
We need to load n, here n is the number of camera, filename containing the x and y coordinates on the image. We need to load n, here n is the number of camera, filename containing the x and y coordinates on the image.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
import numpy as np import numpy as np
from navipy.arenatools.triangulate import triangulate_ncam_pairwise from navipy.arenatools.triangulate import triangulate_ncam_pairwise
from navipy.io.ivfile import load as ivload from navipy.io.ivfile import load as ivload
from navipy.io.opencv import load_cameras_calibration from navipy.io.opencv import load_cameras_calibration
import pkg_resources import pkg_resources
# Use the trafile from the resources # Use the trafile from the resources
# You can adapt this code, by changing trajfile # You can adapt this code, by changing trajfile
# with your own trajectory file # with your own trajectory file
trajfile_side = pkg_resources.resource_filename( trajfile_side = pkg_resources.resource_filename(
'navipy', 'navipy',
'resources/sample_experiment/Lobecke_JEB_2018/Y101_OBFlight_0001_side.tra') 'resources/sample_experiment/Lobecke_JEB_2018/Y101_OBFlight_0001_side.tra')
trajfile_top = pkg_resources.resource_filename( trajfile_top = pkg_resources.resource_filename(
'navipy', 'navipy',
'resources/sample_experiment/Lobecke_JEB_2018/Y101_OBFlight_0001_top.tra') 'resources/sample_experiment/Lobecke_JEB_2018/Y101_OBFlight_0001_top.tra')
calibfile = pkg_resources.resource_filename( calibfile = pkg_resources.resource_filename(
'navipy', 'navipy',
'resources/sample_experiment/Lobecke_JEB_2018/fullcalib_fromtra.xml') 'resources/sample_experiment/Lobecke_JEB_2018/fullcalib_fromtra.xml')
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
Load the calibration from an xml format (opencv like format) Load the calibration from an xml format (opencv like format)
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
calib = load_cameras_calibration(calibfile) calib = load_cameras_calibration(calibfile)
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
Load the trajectory as seen from the top and side camera Load the trajectory as seen from the top and side camera
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
trajdata_side = ivload(trajfile_side) trajdata_side = ivload(trajfile_side)
trajdata_top = ivload(trajfile_top) trajdata_top = ivload(trajfile_top)
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
Format the data for triangulation Format the data for triangulation
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
nframes = trajdata_side.shape[0] nframes = trajdata_side.shape[0]
ncameras = len(calib) ncameras = len(calib)
ncoordinates = 2 # Always two because a camera is a plane ncoordinates = 2 # Always two because a camera is a plane
mark_i = 0 mark_i = 0
pts_cam = np.zeros((ncameras, nframes, ncoordinates)) pts_cam = np.zeros((ncameras, nframes, ncoordinates))
#First camera #First camera
pts_cam[1,:,0]=trajdata_top.loc[:, mark_i].x pts_cam[1,:,0]=trajdata_top.loc[:, mark_i].x
pts_cam[1,:,1]=trajdata_top.loc[:, mark_i].y pts_cam[1,:,1]=trajdata_top.loc[:, mark_i].y
#Second camera #Second camera
pts_cam[0,:,0]=trajdata_side.loc[:, mark_i].x pts_cam[0,:,0]=trajdata_side.loc[:, mark_i].x
pts_cam[0,:,1]=trajdata_side.loc[:, mark_i].y pts_cam[0,:,1]=trajdata_side.loc[:, mark_i].y
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
and then triangulate. and then triangulate.
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
point_3d, p3dunscaled, validcmb = triangulate_ncam_pairwise(calib, pts_cam) point_3d, p3dunscaled, validcmb = triangulate_ncam_pairwise(calib, pts_cam)
fig = plt.figure() fig = plt.figure()
ax = fig.add_subplot(111, projection='3d') ax = fig.add_subplot(111, projection='3d')
plt.plot(point_3d[:,0],point_3d[:,1],point_3d[:,2]) plt.plot(point_3d[:,0],point_3d[:,1],point_3d[:,2])
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
#### Single value decomposition #### Single value decomposition
## Conversion to navipy trajectories ## Conversion to navipy trajectories
### From Matlab to navipy ### From Matlab to navipy
%% Cell type:code id: tags: %% Cell type:code id: tags:
``` python ``` python
from scipy.io import loadmat from scipy.io import loadmat
import numpy as np import numpy as np
import os import os
from navipy.trajectories import Trajectory from navipy.trajectories import Trajectory
import pkg_resources import pkg_resources
# Use the trafile from the resources # Use the trafile from the resources
# You can adapt this code, by changing trajfile # You can adapt this code, by changing trajfile
# with your own trajectory file # with your own trajectory file
trajfile = pkg_resources.resource_filename( trajfile = pkg_resources.resource_filename(
'navipy', 'navipy',
'resources/sample_experiment/Lobecke_JEB_2018/Y101_OBFlight_0001.mat') 'resources/sample_experiment/Lobecke_JEB_2018/Y101_OBFlight_0001.mat')
csvtrajfile, _ = os.path.splitext(trajfile) csvtrajfile, _ = os.path.splitext(trajfile)
csvtrajfile = csvtrajfile+'.csv' csvtrajfile = csvtrajfile+'.csv'
mymat = loadmat(trajfile) mymat = loadmat(trajfile)
# matlab files are loaded in a dictionary # matlab files are loaded in a dictionary
# we need to identify, under which key the trajectory has been saved # we need to identify, under which key the trajectory has been saved
print(mymat.keys()) print(mymat.keys())
key = 'trajectory' key = 'trajectory'
# Arrays are placed in a tupple. We need to access the level # Arrays are placed in a tupple. We need to access the level
# of the array itself. # of the array itself.
mymat = mymat[key][0][0][0] mymat = mymat[key][0][0][0]
# The array should be a numpy array, and therefore as the .shape function # The array should be a numpy array, and therefore as the .shape function
# to display the array size: # to display the array size:
print(mymat.shape) print(mymat.shape)
# In this example the array has 7661 rows and 4 columns # In this example the array has 7661 rows and 4 columns
# the columns are: x,y,z, yaw. Therefore pitch and roll were assumed to be constant (here null) # the columns are: x,y,z, yaw. Therefore pitch and roll were assumed to be constant (here null)
# We can therefore init a trajectory with the convention for rotation # We can therefore init a trajectory with the convention for rotation
# often yaw-pitch-roll (i.e. 'rzyx') and with indeces the number of sampling points we have # often yaw-pitch-roll (i.e. 'rzyx') and with indeces the number of sampling points we have
# in the trajectory # in the trajectory
rotconvention = 'rzyx' rotconvention = 'rzyx'
indeces = np.arange(0,mymat.shape[0]) indeces = np.arange(0,mymat.shape[0])
mytraj = Trajectory(rotconv = rotconvention, indeces=indeces) mytraj = Trajectory(rotconv = rotconvention, indeces=indeces)
# We now can assign the values # We now can assign the values
mytraj.x = mymat[:,0] mytraj.x = mymat[:,0]
mytraj.y = mymat[:,1] mytraj.y = mymat[:,1]
mytraj.z = mymat[:,2] mytraj.z = mymat[:,2]
mytraj.alpha_0 = -mymat[:,3] # - because ivTrace mytraj.alpha_0 = -mymat[:,3] # - because ivTrace
mytraj.alpha_1 = 0 mytraj.alpha_1 = 0
mytraj.alpha_2 = 0 mytraj.alpha_2 = 0
# We can then save mytraj as csv file for example # We can then save mytraj as csv file for example
mytraj.to_csv(csvtrajfile) mytraj.to_csv(csvtrajfile)
mytraj.head() mytraj.head()
``` ```
%% Cell type:markdown id: tags: %% Cell type:markdown id: tags:
### From csv to navipy ### From csv to navipy
Navipy can read csv files with 7 columns and a two line headers Navipy can read csv files with 7 columns and a two line headers
<table> <table>
<tr> <tr>
<td>location</td> <td>location</td>
<td>location</td> <td>location</td>
<td>location</td> <td>location</td>
<td>rzyx</td> <td>rzyx</td>
<td>rzyx</td> <td>rzyx</td>
<td>rzyx</td> <td>rzyx</td>
</tr> </tr>
<tr> <tr>
<td>x</td> <td>x</td>
<td>y</td> <td>y</td>
<td>z</td> <td>z</td>
<td>alpha_0</td> <td>alpha_0</td>
<td>alpha_1</td> <td>alpha_1</td>
<td>alpha_2</td> <td>alpha_2</td>
</tr> </tr>
</table> </table>
But you may have csv file with a different format. But you may have csv file with a different format.
Here we are going to show, how to convert your csv file format to the one required by navipy Here we are going to show, how to convert your csv file format to the one required by navipy
......
navipy/arenatools/cam_dlt.py
+ 136
− 23
View file @ 0a5087d5
...@@ -101,6 +101,53 @@ units are [u,v] i.e. camera coordinates or pixels ...@@ -101,6 +101,53 @@ units are [u,v] i.e. camera coordinates or pixels
return xyz, rmse return xyz, rmse
def dlt_principal_point(coeff):
normalisation = np.sum(coeff[8:11]**2)
u_0 = np.sum(coeff[0:3]*coeff[8:11])/normalisation
v_0 = np.sum(coeff[4:7]*coeff[8:11])/normalisation
return u_0, v_0
def dlt_principal_distance(coeff):
normalisation = np.sum(coeff[8:11]**2)
return 1/np.sqrt(normalisation)
def dlt_scale_factors(coeff):
u_0, v_0 = dlt_principal_point(coeff)
normalisation = np.sum(coeff[8:11]**2)
du = (u_0*coeff[8] - coeff[0])**2
du += (u_0*coeff[9] - coeff[1])**2
du += (u_0*coeff[10] - coeff[2])**2
du /= normalisation
dv = (v_0*coeff[8] - coeff[4])**2
dv += (v_0*coeff[9] - coeff[5])**2
dv += (v_0*coeff[10] - coeff[6])**2
dv /= normalisation
return du, dv
def dlt_transformation_matrix(coeff):
u_0, v_0 = dlt_principal_point(coeff)
d = dlt_principal_distance(coeff)
du, dv = dlt_scale_factors(coeff)
transform = np.zeros((3, 3))
transform[0, 0] = (u_0*coeff[8]-coeff[0])/du
transform[0, 1] = (u_0*coeff[9]-coeff[1])/du
transform[0, 2] = (u_0*coeff[10]-coeff[2])/du
transform[1, 0] = (v_0*coeff[8]-coeff[4])/dv
transform[1, 1] = (v_0*coeff[9]-coeff[5])/dv
transform[1, 2] = (v_0*coeff[10]-coeff[6])/dv
transform[2, 0] = coeff[8]
transform[2, 1] = coeff[9]
transform[2, 2] = coeff[10]
return d*transform
def dlt_inverse(coeff, frames): def dlt_inverse(coeff, frames):
""" """
This function reconstructs the pixel coordinates of a 3D coordinate as This function reconstructs the pixel coordinates of a 3D coordinate as
...@@ -123,10 +170,68 @@ def dlt_inverse(coeff, frames): ...@@ -123,10 +170,68 @@ def dlt_inverse(coeff, frames):
coeff[5]+frames[:, 2]*coeff[6]+coeff[7] coeff[5]+frames[:, 2]*coeff[6]+coeff[7]
uv[:, 0] /= normalisation uv[:, 0] /= normalisation
uv[:, 1] /= normalisation uv[:, 1] /= normalisation
# Apply distortion
delta_uv = np.zeros((frames.shape[0], 2))
u_0, v_0 = dlt_principal_point(coeff)
zeta = uv[:, 0] - u_0
eta = uv[:, 1] - v_0
rsq = zeta**2 + eta**2
if coeff.shape[0] > 11:
delta_uv[:, 0] += zeta*(coeff[11]*rsq)
delta_uv[:, 1] += eta*(coeff[11]*rsq)
if coeff.shape[0] > 13:
delta_uv[:, 0] += zeta*(coeff[12]*(rsq**2) + coeff[13]*(rsq**3))
delta_uv[:, 1] += eta*(coeff[12]*(rsq**2) + coeff[13]*(rsq**3))
if coeff.shape[0] > 15:
delta_uv[:, 0] += coeff[14]*(rsq + 2*(zeta**2)) + coeff[15]*eta*zeta
delta_uv[:, 1] += coeff[15]*(rsq + 2*(eta**2)) + coeff[14]*eta*zeta
# print(eta, zeta, rsq)
uv += delta_uv
return uv return uv
def dlt_compute_coeffs(frames, campts): def _dlt_matrices_calib(vframes, vcampts, nparams=11, l1to11=np.zeros(11)):
# re arange the frame matrix to facilitate the linear least
# sqaures solution
if nparams < 11:
nparams = 11
matrix = np.zeros((vframes.shape[0]*2, 16)) # 16 for the dlt
u_0, v_0 = dlt_principal_point(l1to11)
for num_i, index_i in enumerate(vframes.index):
# eta, zeta, Rsq depends on L9to11
zeta = vcampts.loc[index_i, 'u'] - u_0 # -u_0 = 0 ??
eta = vcampts.loc[index_i, 'v'] - v_0 # -v_0 = 0
R = np.sum(l1to11[-3:] * vframes.loc[index_i, ['x', 'y', 'z']])+1
rsq = eta**2 + zeta**2
# populate matrix
matrix[2*num_i, 0:3] = vframes.loc[index_i, ['x', 'y', 'z']]
matrix[2*num_i+1, 4:7] = vframes.loc[index_i, ['x', 'y', 'z']]
matrix[2*num_i, 3] = 1
matrix[2*num_i+1, 7] = 1
matrix[2*num_i, 8:11] = vframes.loc[index_i,
['x', 'y', 'z']]*(-vcampts.loc[index_i, 'u'])
matrix[2*num_i+1, 8: 11] = vframes.loc[index_i,
['x', 'y', 'z']]*(-vcampts.loc[index_i, 'v'])
# 12th parameter
matrix[2*num_i, 11] = zeta*rsq*R
matrix[2*num_i+1, 11] = eta*rsq*R
# 13th and 14th parameters
matrix[2*num_i, 12] = zeta*(rsq**2)*R
matrix[2*num_i+1, 12] = eta*(rsq**2)*R
matrix[2*num_i, 13] = zeta*(rsq**3)*R
matrix[2*num_i+1, 13] = eta*(rsq**3)*R
# 15th and 16th parameters
matrix[2*num_i, 12] = (rsq + 2*(zeta**2))*R
matrix[2*num_i+1, 12] = eta*zeta*R
matrix[2*num_i, 13] = eta*zeta*R
matrix[2*num_i+1, 13] = (rsq + 2*(eta**2))*R
matrix[2*num_i, :] /= R
matrix[2*num_i+1, :] /= R
return matrix[:, : nparams]
def dlt_compute_coeffs(frames, campts, nparams=11, niter=100):
""" """
A basic implementation of 11 parameters DLT A basic implementation of 11 parameters DLT
...@@ -142,36 +247,44 @@ single plane. ...@@ -142,36 +247,44 @@ single plane.
# remove NaNs # remove NaNs
valid_idx = frames.dropna(how='any').index valid_idx = frames.dropna(how='any').index
valid_idx = campts.loc[valid_idx, :].dropna(how='any').index valid_idx = campts.loc[valid_idx, :].dropna(how='any').index
# valid df # valid df
vframes = frames.loc[valid_idx, :] vframes = frames.loc[valid_idx, :]
vcampts = campts.loc[valid_idx, :] vcampts = campts.loc[valid_idx, :]
# Get the matrices for calib
# re arange the frame matrix to facilitate the linear least matrix = _dlt_matrices_calib(vframes, vcampts)
# sqaures solution vcampts_f = vcampts.values.flatten() # [u_1, v_1, ... u_n, v_n]
matrix = np.zeros((vframes.shape[0]*2, 11)) # 11 for the dlt
for num_i, index_i in enumerate(vframes.index):
matrix[2*num_i, 0:3] = vframes.loc[index_i, ['x', 'y', 'z']]
matrix[2*num_i+1, 4:7] = vframes.loc[index_i, ['x', 'y', 'z']]
matrix[2*num_i, 3] = 1
matrix[2*num_i+1, 7] = 1
matrix[2*num_i, 8:11] = \
vframes.loc[index_i, ['x', 'y', 'z']]*(-vcampts.loc[index_i, 'u'])
matrix[2*num_i+1, 8:11] = \
vframes.loc[index_i, ['x', 'y', 'z']]*(-vcampts.loc[index_i, 'v'])
# re argen the campts array for the linear solution
vcampts_f = np.reshape(np.flipud(np.rot90(vcampts)), vcampts.size, 1)
print(vcampts_f.shape)
print(matrix.shape)
# get the linear solution the 11 parameters # get the linear solution the 11 parameters
coeff = np.linalg.lstsq(matrix, vcampts_f, rcond=None)[0] coeff = np.linalg.lstsq(matrix, vcampts_f, rcond=None)[0]
# compute the position of the frame in u,v coordinates given the linear # compute the position of the frame in u,v coordinates given the linear
# solution from the previous line # solution from the previous line
matrix_uv = dlt_inverse(coeff, vframes) matrix_uv = dlt_inverse(coeff, vframes)
# compute the rmse between the ideal frame u,v and the # compute the rmse between the ideal frame u,v and the
# recorded frame u,v # recorded frame u,v
rmse = np.sqrt(np.mean(np.sum((matrix_uv-vcampts)**2))) rmse = np.sqrt(np.mean(np.sum((matrix_uv-vcampts)**2)))
return coeff, rmse if nparams == 11:
return coeff, rmse
# Now we can try to guess the other coefficients
if nparams in [12, 14, 16]:
for _ in range(niter):
# 9th to 11th parameters are index 8 to 10 (0 being the 1st param)
l1to11 = coeff[: 11]
# Get the matrices for calib
matrix = _dlt_matrices_calib(vframes, vcampts,
nparams=nparams, l1to11=l1to11)
vcampts_normed = vcampts_f.copy()
for num_i, index_i in enumerate(vframes.index):
normalisation = np.sum(
l1to11[-3:] * vframes.loc[index_i, ['x', 'y', 'z']])+1
vcampts_normed[2*num_i] /= normalisation
vcampts_normed[2*num_i + 1] /= normalisation
coeff = np.linalg.lstsq(matrix, vcampts_normed, rcond=None)[0]
print(coeff)
# compute the position of the frame in u,v coordinates given the linear
# solution from the previous line
matrix_uv = dlt_inverse(coeff, vframes)
# compute the rmse between the ideal frame u,v and the
# recorded frame u,v
rmse = np.sqrt(np.mean(np.sum((matrix_uv-vcampts)**2)))
return coeff, rmse
else:
raise ValueError('nparams can be either [11,12,14,16]')
navipy/scripts/dlt_calibrator.py
+ 25
− 1
View file @ 0a5087d5
...@@ -5,6 +5,10 @@ import cv2 ...@@ -5,6 +5,10 @@ import cv2
import os import os
from navipy.arenatools.cam_dlt import dlt_inverse from navipy.arenatools.cam_dlt import dlt_inverse
from navipy.arenatools.cam_dlt import dlt_compute_coeffs from navipy.arenatools.cam_dlt import dlt_compute_coeffs
from navipy.arenatools.cam_dlt import dlt_principal_point
from navipy.arenatools.cam_dlt import dlt_principal_distance
from navipy.arenatools.cam_dlt import dlt_scale_factors
from navipy.arenatools.cam_dlt import dlt_transformation_matrix
keybinding = dict() keybinding = dict()
...@@ -42,6 +46,16 @@ def parser_dlt_calibrator(): ...@@ -42,6 +46,16 @@ def parser_dlt_calibrator():
parser.add_argument('-s', '--scale', parser.add_argument('-s', '--scale',
default=0.5, default=0.5,
help=arghelp) help=arghelp)
arghelp = 'number of dlt parameters (coeff)'
parser.add_argument('-c', '--ndltcoeff',
type=int,
default=11,
help=arghelp)
arghelp = 'number of iteration for calibration'
parser.add_argument('-e', '--epoque',
type=int,
default=100,
help=arghelp)
return parser return parser
...@@ -71,6 +85,8 @@ def main(): ...@@ -71,6 +85,8 @@ def main():
# The image may need to be scale because screen may have # The image may need to be scale because screen may have
# less pixel than the image # less pixel than the image
scale = args['scale'] scale = args['scale']
ndlt_coeff = args['ndltcoeff']
niter = args['epoque']
imageref = cv2.imread(args['image']) imageref = cv2.imread(args['image'])
# define some constants # define some constants
showframe_ref = 50 showframe_ref = 50
...@@ -144,9 +160,17 @@ def main(): ...@@ -144,9 +160,17 @@ def main():
break break
if key == ord("c"): if key == ord("c"):
print('calibrate') print('calibrate')
coeff, rmse = dlt_compute_coeffs(frames, campts) coeff, rmse = dlt_compute_coeffs(
frames, campts, nparams=ndlt_coeff,
niter=niter)
print(rmse) print(rmse)
print(coeff) print(coeff)
print('principal points: {}'.format(dlt_principal_point(coeff)))
print('principal distance: {}'.format(
dlt_principal_distance(coeff)))
print('scale factor: {}'.format(dlt_scale_factors(coeff)))
print('transform:')
print(dlt_transformation_matrix(coeff))
coeff = pd.Series(data=coeff) coeff = pd.Series(data=coeff)
coeff.to_csv(os.path.splitext(args['points'])[0]+'_coeff.csv') coeff.to_csv(os.path.splitext(args['points'])[0]+'_coeff.csv')
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment