For my project I was trying 3D LiDAR to Camera projection. I was recommended MATLAB LiDAR-Camera modules for calibration and then use the results for the projection on a live stream of the data.
In MATLAB, I got the rotation matrix ( R ), the translation matrix ( T ) and the camera intrinsic matrix ( M ). When using the MATLAB tools I am getting the results as follows :
MATLAB generated projection for the same values of R, T and M
But using the literature in which the projection matrix is given as
( M 0 )x( [[ R T ], [ 0 1]] ) to get from a [x y z 1] to [u v w], I am getting the following result :
Projection of M, R and T as per the literature
M, R,T as calculated from MATLAB
M = array([[904.4679, 0. , 596.9176],
[ 0. , 814.7088, 349.8212],
[ 0. , 0. , 1. ]])
R = array([[ 0.124 , -0.0038, 0.9923],
[-0.9912, 0.0474, 0.124 ],
[-0.0475, -0.9989, 0.0021]])
T = array([[-0.56 , 0.241 , -0.4454]])
A code snippet looks like this
rotation = np.array([[0.1240,-0.0038,0.9923], [-0.9912,0.0474,0.1240],[-0.0475,-0.9989,0.0021]])
traslation = np.array([[-0.5600,0.2410,-0.4454]])
traslation_1 = np.array([[-0.4454,0.2410,-0.5600]])
intrinsic = np.array([[904.4679,0,596.9176],[0,814.7088,349.8212],[0,0,1]])
a = np.concatenate((rotation,np.array([[0,0,0]])), axis =0)
b = np.concatenate((traslation_1.T, np.array([[1]])), axis =0)
c = np.concatenate((a,b), axis =1)
print('\n Extrinsic:\n \n',c)
d = np.concatenate((intrinsic, np.array([[0,0,0]]).T), axis =1)
print('\n Intrinsic:\n \n',d)
e = np.matmul(d,c)
print("\n Final:\n \n", e)
df = pd.read_csv('out_file.csv')
img = cv2.imread('images/0001.png')
v1_max = 0
v2_max = 0
uv = []
import matplotlib.pyplot as plt
for i in range(df.shape[0]):
point = np.array([[df['x'].iloc[i],df['y'].iloc[i],df['z'].iloc[i],1]]).T
v = np.matmul(e, point)
v = v/v[2]
if v[0]<=720 and v[0] > 0 and v[1] < 1280 and v[1] > 0:
img[int(np.floor(v[0])),int(np.floor(v[1]))] = [0,255,255]
cv2.imwrite('file.png', img)
Hardware configuration :
Velodyne 64 Channel LiDAR # 10 HZ
Camera : 1280*720 monocular camera
Checkerboard : 10*7 with 10 cm of pattern and a padding .
I need some help as to how I cam take the parameters from the MATLAB and do the same in the scripts and get the near results .
I am expecting as to where I am going wrong when projecting the LiDAR points onto the camera, as to when I am using MATLAB functions I am getting a correct projection. Same is not there with the literature.
Related
I am trying to use interpn (in python using Scipy) to replicate results from Matlab using interp3. However, I am struggling to structure my arguments. I tried the following line:
f = interpn(blur_maps, fx, fy, pyr_level)
Where blur maps is a 600 x 800 x 7 representing a grayscale image at seven levels of blur,
fx and fy are indices of the seven maps. Both fx and fy are 2d arrays. pyr_level is a 2d array that contains values from 1 to 7 representing the blur map to be interpolated.
My question is since I incorrectly arranged the arguments, how can I arrange them in a way that works? I tried to look up examples but I didn't see anything similar. Here is an example of the data I am trying to interpolate:
import numpy as np
import cv2, math
from scipy.interpolate import interpn
levels = 7
img_path = '/Users/alimahdi/Desktop/i4.jpg'
img = cv2.cvtColor(cv2.imread(img_path), cv2.COLOR_BGR2GRAY)
row, col = img.shape
x_range = np.arange(0, col)
y_range = np.arange(0, row)
fx, fy = np.meshgrid(x_range, y_range)
e = np.exp(np.sqrt(fx ** 2 + fy ** 2))
pyr_level = 7 * (e - np.min(e)) / (np.max(e) - np.min(e))
blur_maps = np.zeros((row, col, levels))
blur_maps[:, :, 0] = img
for i in range(levels - 1):
img = cv2.pyrDown(img)
r, c = img.shape
tmp = img
for j in range(int(math.log(row / r, 2))):
tmp = cv2.pyrUp(tmp)
blur_maps[:, :, i + 1] = tmp
pixelGrid = [np.arange(x) for x in blur_maps.shape]
interpPoints = np.array([fx.flatten(), fy.flatten(), pyr_level.flatten()])
interpValues = interpn(pixelGrid, blur_maps, interpPoints.T)
finalValues = np.reshape(interpValues, fx.shape)
I am now getting the following error: ValueError: One of the requested xi is out of bounds in dimension 0 I do know that the problem is in interpPoints but I am not sure how to fix it. Any suggestions?
The documentation for scipy.interpolate.interpn states that the first argument is a grid of the data you are interpolating over (which is just the integers of the pixel numbers), second argument is data (blur_maps) and third arguments is the interpolation points in the form (npoints, ndims). So you would have to do something like:
import scipy.interpolate
pixelGrid = [np.arange(x) for x in blur_maps.shape] # create grid of pixel numbers as per the docs
interpPoints = np.array([fx.flatten(), fy.flatten(), pyr_level.flatten()])
# interpolate
interpValues = scipy.interpolate.interpn(pixelGrid, blur_maps, interpPoints.T)
# now reshape the output array to get in the original format you wanted
finalValues = np.reshape(interpValues, fx.shape)
I have a Python-Code and want to rewrite it in Octave, but I meet so many problems during the converting. I found a solution for some of them and some of them still need your help. Now i would start with this part of the code :
INVOLUTE_FI = 0
INVOLUTE_FO = 1
INVOLUTE_OI = 2
INVOLUTE_OO = 3
def coords_inv(phi, geo, theta, inv):
"""
Coordinates of the involutes
Parameters
----------
phi : float
The involute angle
geo : struct
The structure with the geometry obtained from get_geo()
theta : float
The crank angle, between 0 and 2*pi
inv : int
The key for the involute to be considered
"""
rb = geo.rb
ro = rb*(pi - geo.phi_fi0 + geo.phi_oo0)
Theta = geo.phi_fie - theta - pi/2.0
if inv == INVOLUTE_FI:
x = rb*cos(phi)+rb*(phi-geo.phi_fi0)*sin(phi)
y = rb*sin(phi)-rb*(phi-geo.phi_fi0)*cos(phi)
elif inv == INVOLUTE_FO:
x = rb*cos(phi)+rb*(phi-geo.phi_fo0)*sin(phi)
y = rb*sin(phi)-rb*(phi-geo.phi_fo0)*cos(phi)
elif inv == INVOLUTE_OI:
x = -rb*cos(phi)-rb*(phi-geo.phi_oi0)*sin(phi)+ro*cos(Theta)
y = -rb*sin(phi)+rb*(phi-geo.phi_oi0)*cos(phi)+ro*sin(Theta)
elif inv == INVOLUTE_OO:
x = -rb*cos(phi)-rb*(phi-geo.phi_oo0)*sin(phi)+ro*cos(Theta)
y = -rb*sin(phi)+rb*(phi-geo.phi_oo0)*cos(phi)+ro*sin(Theta)
else:
raise ValueError('flag not valid')
return x,y
def CVcoords(CVkey, geo, theta, N = 1000):
"""
Return a tuple of numpy arrays for x,y coordinates for the lines which
determine the boundary of the control volume
Parameters
----------
CVkey : string
The key for the control volume for which the polygon is desired
geo : struct
The structure with the geometry obtained from get_geo()
theta : float
The crank angle, between 0 and 2*pi
N : int
How many elements to include in each entry in the polygon
Returns
-------
x : numpy array
X-coordinates of the outline of the control volume
y : numpy array
Y-coordinates of the outline of the control volume
"""
Nc1 = Nc(theta, geo, 1)
Nc2 = Nc(theta, geo, 2)
if CVkey == 'sa':
r = (2*pi*geo.rb-geo.t)/2.0
xee,yee = coords_inv(geo.phi_fie,geo,0.0,'fi')
xse,yse = coords_inv(geo.phi_foe-2*pi,geo,0.0,'fo')
xoie,yoie = coords_inv(geo.phi_oie,geo,theta,'oi')
xooe,yooe = coords_inv(geo.phi_ooe,geo,theta,'oo')
x0,y0 = (xee+xse)/2,(yee+yse)/2
beta = atan2(yee-y0,xee-x0)
t = np.linspace(beta,beta+pi,1000)
x,y = x0+r*np.cos(t),y0+r*np.sin(t)
return np.r_[x,xoie,xooe,x[0]],np.r_[y,yoie,yooe,y[0]]
https://docs.scipy.org/doc/numpy/reference/generated/numpy.r_.html I just don´t understand the last Output, and I am still confuse what´s mean _r here, and how can I write it by Octave?....I read what is written in the link, but it still not clear for me.
return np.r_[x,xoie,xooe,x[0]], np.r_[y,yoie,yooe,y[0]]
The function returns 2 values, both arrays created by np.r_.
np.r_[....] has indexing syntax, and ends up being translated into a function call to the np.r_ object. The result is just the concatenation of the arguments:
In [355]: np.r_[1, 3, 6:8, np.array([3,2,1])]
Out[355]: array([1, 3, 6, 7, 3, 2, 1])
With the [] notation it can accept slice like objects (6:8) though I don't see any of those here. I'd have to study the rest of the code to identify whether the other arguments are scalars (single values) or arrays.
My Octave is rusty (though I could experiment with the conversion).
t = np.lispace... # I think that exists in Octave, a 1000 values
x = x0+r*np.cos(t) # a derived array of 1000 values
xoie one of the values returned by coords_inv; may be scalar or array. x[0] the first value of x. So the r_ probably produces a 1d array made up of x, and the subsequent values.
I have a logical matrix:
0,0,0,0,0,0,0,0,0,0,0
0,0,0,0,0,0,0,0,0,0,0
0,0,0,1,1,1,1,1,0,0,0
0,0,0,1,1,1,1,1,1,0,0
0,0,1,1,1,1,1,1,1,0,0
0,0,1,1,1,1,1,1,1,0,0
0,0,1,1,1,1,1,1,1,0,0
0,0,0,1,1,1,1,1,1,0,0
0,0,0,0,1,1,0,0,0,0,0
0,0,0,0,0,0,0,0,0,0,0
0,0,0,0,0,0,0,0,0,0,0
and I would like to fit an ellipse as best as possible and calculate the error. The error could be the elementwise difference between the original data and the best ellipse found.
I have seen the following method: MATLAB Curve Fitting (Ellipse-like)
but I am not sure that is the shortest way to do it.
How would you recommend to find the closest elliptical logical matrix?
Find the ellipse 6 DOF parameters:
%get edge locations
md = m-imerode(m,ones(3));
md = padarray(md,10);axis equal
%find edges x,y
[y,x] = find(md);
%fit to Q = a*x^2 + b*y^2 + 2*c*x*y + 2*d*x + 2*e*y + f
H=[x.*x y.*y 2*x.*y x y x*0+1];
[v,g]=eig(H'*H);
a = v(1,1);
b = v(2,1);
c = v(3,1);
d = v(4,1);
e = v(5,1);
f = v(6,1);
[xg, yg] = meshgrid(1:size(md,2),1:size(md,1));
ev = a*xg.^2+b*yg.^2+2*c*xg.*yg+d*xg+e*yg+f;
imagesc(ev);axis equal
I would fist extract the boundary of your logical ellipse. Iterate the following function from i=2 to n-1, j=2 to m-1, for [n,m]=size(M). It will give you the "boundary matrix", where the boundary is represented by 1 and 0 for others. Then use the method described in here to get the coefficients of the ellipse equation. Note that the matrix indices start from top-left. So you may need to modify the index ordering depending on where you want the origin to be.
function [ bd ] = logical_neighbor( loM, i, j )
bd=0;
if loM(i,j) == 0
return;
else
for ni=1:3
for nj=1:3
if loM(i-2+ni,j-2+nj) == 0
bd= 1;
return;
end
end
end
end
end
I'm trying to get the RGB values of a pixel of an image chosen by the user, but then I get this out of bounds error. This is the code:
x = int16(zeros(10,1));
y = int16(zeros(10,1));
imshow(img);
[x,y] = ginput;
disp(['x = ' num2str(x)]);
disp(['y= ' num2str(y)]);
r = img(x,y,1);
g = img(x,y,2);
b = img(x,y,3);
And this is the error I get (when the user chooses the pixel 120,131):
Attempted to access img(120,131,1);
index must be a positive integer or logical.
To stop having this error I declared x and y as a int16, but the error still apeared. Then I tried to put the value of the pixel manually so I did something like this:
r = img(229,104,1);
And the error now is this:
Attempted to access img(229,104,1); index out of
bounds because size(img)=[217,331,3].
How is this possible If the pixel isn't really out of bounds? What is the problem with the code?
You are confusing X-Y coordinates with row-column coordinates.
ginput returs x ( = column ), y ( = row ) coordinates of a pixel.
When accessing the matrix img( ?, ?, : ) you need to provide row ( = y ), column ( - x ) coordinates:
>> img( y, x, 1 )
In addition to Shai's answer, I'm also wondering if the call to ginput doesn't overwrite the data type declared as int16 for x and y? If I do something similar in Octave, I get:
>> [x,y]=ginput
x = 79.798
y = 72.042
I would suggest:
r = img(round(y),round(x),1);
g = img(round(y),round(x),2);
b = img(round(y),round(x),3);
This is easy to do in OpenCV however I would like a native Matlab implementation that is fairly efficient and can be easily changed. The method should be able to take the camera parameters as specified in the above link.
The simplest and most common way of doing undistort (also called unwarp or compensating for lens distortion) is to do a forward distortion on a chosen output photo size and then a reverse mapping using bilinear interpolation.
Here is code I wrote for performing this:
function I = undistort(Idistorted, params)
fx = params.fx;
fy = params.fy;
cx = params.cx;
cy = params.cy;
k1 = params.k1;
k2 = params.k2;
k3 = params.k3;
p1 = params.p1;
p2 = params.p2;
K = [fx 0 cx; 0 fy cy; 0 0 1];
I = zeros(size(Idistorted));
[i j] = find(~isnan(I));
% Xp = the xyz vals of points on the z plane
Xp = inv(K)*[j i ones(length(i),1)]';
% Now we calculate how those points distort i.e forward map them through the distortion
r2 = Xp(1,:).^2+Xp(2,:).^2;
x = Xp(1,:);
y = Xp(2,:);
x = x.*(1+k1*r2 + k2*r2.^2) + 2*p1.*x.*y + p2*(r2 + 2*x.^2);
y = y.*(1+k1*r2 + k2*r2.^2) + 2*p2.*x.*y + p1*(r2 + 2*y.^2);
% u and v are now the distorted cooridnates
u = reshape(fx*x + cx,size(I));
v = reshape(fy*y + cy,size(I));
% Now we perform a backward mapping in order to undistort the warped image coordinates
I = interp2(Idistorted, u, v);
To use it one needs to know the camera parameters of the camera being used.
I am currently using the PMD CamboardNano which according to the Cayim.com forums has the parameters used here:
params = struct('fx',104.119, 'fy', 103.588, 'cx', 81.9494, 'cy', 59.4392, 'k1', -0.222609, 'k2', 0.063022, 'k3', 0, 'p1', 0.002865, 'p2', -0.001446);
I = undistort(Idistorted, params);
subplot(121); imagesc(Idistorted);
subplot(122); imagesc(I);
Here is an example of the output from the Camboard Nano. Note: I artificially added border lines to see what the effect was of the distortion close to the edges (its much more pronounced):
You can now do that as of release R2013B, using the Computer Vision System Toolbox. There is a GUI app called Camera Calibrator and a function undistortImage.