Related
I'm plotting a 3D network using Mayavi,
edge_size = 0.2
pts = mlab.points3d(x, y, z,
scale_mode='none',
scale_factor=0.1)
pts.mlab_source.dataset.lines = np.array(graph.edges())
tube = mlab.pipeline.tube(pts, tube_radius=edge_size)
I want to change edge/tube radius. So I tried
tube = mlab.pipeline.tube(pts, tube_radius=listofedgeradius)
I get an error that says,
traits.trait_errors.TraitError: The 'tube_radius' trait of a TubeFactory instance must be a float
From the error, I understand a list cannot be assigned to tube_radius. In this case, I am not sure how to assign a different radius to each edge.
Any suggestions on how to assign edge weights/edge radius will be helpful.
EDIT: Complete working example
import networkx as nx
import matplotlib.pyplot as plt
import numpy as np
from mayavi import mlab
def main(edge_color=(0.8, 0.8, 0.8), edge_size=0.02):
t = [1, 2, 3, 4, 5]
h = [2, 3, 4, 5, 6]
ed_ls = [(x, y) for x, y in zip(t, h)]
G = nx.OrderedGraph()
G.add_edges_from(ed_ls)
nx.draw(G)
plt.show()
graph_pos = nx.spring_layout(G, dim=3)
# numpy array of x,y,z positions in sorted node order
xyz = np.array([graph_pos[v] for v in sorted(G)])
mlab.figure(1)
mlab.clf()
pts = mlab.points3d(xyz[:, 0], xyz[:, 1], xyz[:, 2])
pts.mlab_source.dataset.lines = np.array(G.edges())
tube = mlab.pipeline.tube(pts, tube_radius=edge_size)
mlab.pipeline.surface(tube, color=edge_color)
mlab.show() # interactive window
main()
New edge weights to be added in the expected output:
listofedgeradius = [1, 2, 3, 4, 5]
tube = mlab.pipeline.tube(pts, tube_radius=listofedgeradius)
Is seems to me that you can't plot multiple tubes with different diameter at once.
So one solution is to plot them one after another:
import networkx as nx
import matplotlib.pyplot as plt
import numpy as np
from mayavi import mlab
def main(edge_color=(0.8, 0.8, 0.8)):
t = [1, 2, 4, 4, 5, 3, 5]
h = [2, 3, 6, 5, 6, 4, 1]
ed_ls = [(x, y) for x, y in zip(t, h)]
G = nx.OrderedGraph()
G.add_edges_from(ed_ls)
graph_pos = nx.spring_layout(G, dim=3)
print(graph_pos)
# numpy array of x,y,z positions in sorted node order
xyz = np.array([graph_pos[v] for v in sorted(G)])
listofedgeradius = np.array([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0]) * 0.1
for i, e in enumerate(G.edges()):
# node number of the edge
i1, i2 = e
# graph_pos is a dictionary
c1 = graph_pos[i1]
c2 = graph_pos[i2]
edge_xyz = np.vstack((c1, c2))
pts = mlab.points3d(edge_xyz[:, 0], edge_xyz[:, 1], edge_xyz[:, 2])
#pts.mlab_source.dataset.lines = np.array(G.edges())
# always first and second point
pts.mlab_source.dataset.lines = np.array([[0, 1]])
tube = mlab.pipeline.tube(pts, tube_radius=listofedgeradius[i])
mlab.pipeline.surface(tube, color=edge_color)
mlab.gcf().scene.parallel_projection = True
mlab.show() # interactive window
main()
Here is a larger example with 100 edges (image below) and one caveat of this solution becomes obvious: the for loop is slow.
import networkx as nx
import matplotlib.pyplot as plt
import numpy as np
from mayavi import mlab
def main(edge_color=(0.8, 0.8, 0.8)):
n = 100
t = np.random.randint(100, size=n)
h = np.random.randint(100, size=n)
ed_ls = [(x, y) for x, y in zip(t, h)]
G = nx.OrderedGraph()
G.add_edges_from(ed_ls)
graph_pos = nx.spring_layout(G, dim=3)
print(graph_pos)
# numpy array of x,y,z positions in sorted node order
xyz = np.array([graph_pos[v] for v in sorted(G)])
listofedgeradius = np.random.rand(n) * 0.01
for i, e in enumerate(G.edges()):
print(i)
# node number of the edge
i1, i2 = e
# graph_pos is a dictionary
c1 = graph_pos[i1]
c2 = graph_pos[i2]
edge_xyz = np.vstack((c1, c2))
pts = mlab.points3d(edge_xyz[:, 0], edge_xyz[:, 1], edge_xyz[:, 2])
#pts.mlab_source.dataset.lines = np.array(G.edges())
# always first and second point
pts.mlab_source.dataset.lines = np.array([[0, 1]])
tube = mlab.pipeline.tube(pts, tube_radius=listofedgeradius[i])
mlab.pipeline.surface(tube, color=edge_color)
mlab.gcf().scene.parallel_projection = True
mlab.show() # interactive window
main()
Inspired by this, this and this I put together a first example that works well for large graphs (I tried up to 5000 edges). There is still a for loop, but it is not used for plotting, only for gathering the data in numpy arrays, so it's not that bad.
import networkx as nx
import matplotlib.pyplot as plt
import numpy as np
from mayavi import mlab
def main(edge_color=(0.8, 0.8, 0.8)):
n = 5000
t = np.random.randint(100, size=n)
h = np.random.randint(100, size=n)
ed_ls = [(x, y) for x, y in zip(t, h)]
G = nx.OrderedGraph()
G.add_edges_from(ed_ls)
graph_pos = nx.spring_layout(G, dim=3)
print(graph_pos)
listofedgeradius = np.random.rand(n) * 0.01
# We create a list of positions and connections, each describing a line.
# We will collapse them in one array before plotting.
x = list()
y = list()
z = list()
s = list()
connections = list()
N = 2 # every edge brings two nodes
# The index of the current point in the total amount of points
index = 0
for i, e in enumerate(G.edges()):
# node number of the edge
i1, i2 = e
# graph_pos is a dictionary
c1 = graph_pos[i1]
c2 = graph_pos[i2]
edge_xyz = np.vstack((c1, c2))
x.append(edge_xyz[:, 0])
y.append(edge_xyz[:, 1])
z.append(edge_xyz[:, 2])
s.append(listofedgeradius[i])
s.append(listofedgeradius[i])
# This is the tricky part: in a line, each point is connected
# to the one following it. We have to express this with the indices
# of the final set of points once all lines have been combined
# together, this is why we need to keep track of the total number of
# points already created (index)
ics = np.vstack(
[np.arange(index, index + N - 1.5),
np.arange(index + 1, index + N - .5)]
).T
#print(ics)
connections.append(ics)
index += N
# Now collapse all positions, scalars and connections in big arrays
x = np.hstack(x)
y = np.hstack(y)
z = np.hstack(z)
s = np.hstack(s)
# print(x.shape)
# print(y.shape)
# print(z.shape)
# print(s.shape)
connections = np.vstack(connections)
# # graph_pos is a dictionary
# c1 = graph_pos[i1]
# c2 = graph_pos[i2]
# edge_xyz = np.vstack((c1, c2))
#src = mlab.points3d(x, y, z, s)
#src = mlab.pipeline.scalar_scatter(x, y, z, s)
src = mlab.plot3d(x, y, z, s)
print(src)
print(src.parent)
print(src.parent.parent)
#src.parent.parent.filter.vary_radius = 'vary_radius_by_scalar'
src.parent.parent.filter.vary_radius = 'vary_radius_by_absolute_scalar'
# Connect them
src.mlab_source.dataset.lines = connections
#src.update()
# The stripper filter cleans up connected lines
lines = mlab.pipeline.stripper(src)
# Finally, display the set of lines
#mlab.pipeline.surface(lines, colormap='Accent', line_width=1, opacity=.4)
#tube = mlab.pipeline.tube(src, tube_radius=0.01)
#tube.filter.radius_factor = 1
#tube.filter.vary_radius = 'vary_radius_by_scalar'
#surf = mlab.pipeline.surface(tube, opacity=0.6, color=(0.8,0.8,0))
#t = mlab.plot3d(x, y, z, s, tube_radius=10)
#t.parent.parent.filter.vary_radius = 'vary_radius_by_scalar'
#pts.mlab_source.dataset.lines = np.array(G.edges())
# always first and second point
#pts.mlab_source.dataset.lines = np.array([[0, 1]])
#tube = mlab.pipeline.tube(src, tube_radius=listofedgeradius[i])
#mlab.pipeline.surface(tube, color=edge_color)
# pts = self.scene.mlab.quiver3d(x, y, z, atomsScales, v, w,
# scalars=scalars, mode='sphere', vmin=0.0, vmax=1.0, figure = scene)
# pts.mlab_source.dataset.lines = bonds
# tube = scene.mlab.pipeline.tube(pts, tube_radius=0.01)
# tube.filter.radius_factor = 1
# tube.filter.vary_radius = 'vary_radius_by_scalar'
# surf = scene.mlab.pipeline.surface(tube, opacity=0.6, color=(0.8,0.8,0))
# t = mlab.plot3d(x, y, z, s, tube_radius=10)
#t.parent.parent.filter.vary_radius = 'vary_radius_by_scalar'
# self.plot = self.scene.mlab.plot3d(x, y, z, t,
# tube_radius=self.radius, colormap='Spectral')
# else:
# self.plot.parent.parent.filter.radius = self.radius
mlab.gcf().scene.parallel_projection = True
# And choose a nice view
mlab.view(33.6, 106, 5.5, [0, 0, .05])
mlab.roll(125)
mlab.show()
main()
I looked at this code:
import numpy as np
from matplotlib import pyplot as plt
def dipole(m, r, r0):
"""
Calculation of field B in point r. B is created by a dipole moment m located in r0.
"""
# R = r - r0 - subtraction of elements of vectors r and r0, transposition of array
R = np.subtract(np.transpose(r), r0).T
# Spatial components of r are the outermost axis
norm_R = np.sqrt(np.einsum("i...,i...", R, R)) # einsum - Einsteinova sumace
# Dot product of R and m
m_dot_R = np.tensordot(m, R, axes=1)
# Computation of B
B = 3 * m_dot_R * R / norm_R**5 - np.tensordot(m, 1 / norm_R**3, axes=0)
B *= 1e-7 # abbreviation for B = B * 1e-7, multiplication B of 1e-7, permeability of vacuum: 4\pi * 10^(-7)
# The result is the magnetic field B
return B
X = np.linspace(-1, 1)
Y = np.linspace(-1, 1)
Bx, By = dipole(m=[0, 1], r=np.meshgrid(X, Y), r0=[-0.2,0.8])
plt.figure(figsize=(8, 8))
plt.streamplot(X, Y, Bx, By)
plt.margins(0, 0)
plt.show()
It shows the following figure:
Is it possible to get coordinates of one line of force? I don't understand how it is plotted.
The streamplot returns a container object 'StreamplotSet' with two parts:
lines: a LineCollection of the streamlines
arrows: a PatchCollection containing FancyArrowPatch objects (these are the triangular arrows)
c.lines.get_paths() gives all the segments. Iterating through these segments, their vertices can be examined. When a segment starts where the previous ended, both belong to the same curve. Note that each segment is a short straight line; many segments are used together to form a streamline curve.
The code below demonstrates iterating through the segments. To show what's happening, each segment is converted to an array of 2D points suitable for plt.plot. Default, plt.plot colors each curve with a new color (repeating every 10). The dots show where each of the short straight segments are located.
To find one particular curve, you could hover with the mouse over the starting point, and note the x coordinate of that point. And then test for that coordinate in the code. As an example, the curve that starts near x=0.48 is drawn in a special way.
import numpy as np
from matplotlib import pyplot as plt
from matplotlib import patches
def dipole(m, r, r0):
R = np.subtract(np.transpose(r), r0).T
norm_R = np.sqrt(np.einsum("i...,i...", R, R))
m_dot_R = np.tensordot(m, R, axes=1)
B = 3 * m_dot_R * R / norm_R**5 - np.tensordot(m, 1 / norm_R**3, axes=0)
B *= 1e-7
return B
X = np.linspace(-1, 1)
Y = np.linspace(-1, 1)
Bx, By = dipole(m=[0, 1], r=np.meshgrid(X, Y), r0=[-0.2,0.8])
plt.figure(figsize=(8, 8))
c = plt.streamplot(X, Y, Bx, By)
c.lines.set_visible(False)
paths = c.lines.get_paths()
prev_end = None
start_indices = []
for index, segment in enumerate(paths):
if not np.array_equal(prev_end, segment.vertices[0]): # new segment
start_indices.append(index)
prev_end = segment.vertices[-1]
for i0, i1 in zip(start_indices, start_indices[1:] + [len(paths)]):
# get all the points of the curve that starts at index i0
curve = np.array([paths[i].vertices[0] for i in range(i0, i1)] + [paths[i1 - 1].vertices[-1]])
special_x_coord = 0.48
for i0, i1 in zip(start_indices, start_indices[1:] + [len(paths)]):
# get all the points of the curve that starts at index i0
curve = np.array([paths[i].vertices[0] for i in range(i0, i1)] + [paths[i1 - 1].vertices[-1]])
if abs(curve[0,0] - special_x_coord) < 0.01: # draw one curve in a special way
plt.plot(curve[:, 0], curve[:, 1], '-', lw=10, alpha=0.3)
else:
plt.plot(curve[:, 0], curve[:, 1], '.', ls='-')
plt.margins(0, 0)
plt.show()
I'm using image-segmentation on some images, and sometimes it would be nice to be able to plot the borders of the segments.
I have a 2D NumPy array that I plot with Matplotlib, and the closest I've gotten, is using contour-plotting.
This makes corners in the array, but is otherwise perfect.
Can Matplotlib's contour-function be made to only plot vertical/horizontal lines, or is there some other way to do this?
An example can be seen here:
import matplotlib.pyplot as plt
import numpy as np
array = np.zeros((20, 20))
array[4:7, 3:8] = 1
array[4:7, 12:15] = 1
array[7:15, 7:15] = 1
array[12:14, 13:14] = 0
plt.imshow(array, cmap='binary')
plt.contour(array, levels=[0.5], colors='g')
plt.show()
I wrote some functions to achieve this some time ago, but I would be glad to figure out how it can be done quicker.
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.collections import LineCollection
def get_all_edges(bool_img):
"""
Get a list of all edges (where the value changes from True to False) in the 2D boolean image.
The returned array edges has he dimension (n, 2, 2).
Edge i connects the pixels edges[i, 0, :] and edges[i, 1, :].
Note that the indices of a pixel also denote the coordinates of its lower left corner.
"""
edges = []
ii, jj = np.nonzero(bool_img)
for i, j in zip(ii, jj):
# North
if j == bool_img.shape[1]-1 or not bool_img[i, j+1]:
edges.append(np.array([[i, j+1],
[i+1, j+1]]))
# East
if i == bool_img.shape[0]-1 or not bool_img[i+1, j]:
edges.append(np.array([[i+1, j],
[i+1, j+1]]))
# South
if j == 0 or not bool_img[i, j-1]:
edges.append(np.array([[i, j],
[i+1, j]]))
# West
if i == 0 or not bool_img[i-1, j]:
edges.append(np.array([[i, j],
[i, j+1]]))
if not edges:
return np.zeros((0, 2, 2))
else:
return np.array(edges)
def close_loop_edges(edges):
"""
Combine thee edges defined by 'get_all_edges' to closed loops around objects.
If there are multiple disconnected objects a list of closed loops is returned.
Note that it's expected that all the edges are part of exactly one loop (but not necessarily the same one).
"""
loop_list = []
while edges.size != 0:
loop = [edges[0, 0], edges[0, 1]] # Start with first edge
edges = np.delete(edges, 0, axis=0)
while edges.size != 0:
# Get next edge (=edge with common node)
ij = np.nonzero((edges == loop[-1]).all(axis=2))
if ij[0].size > 0:
i = ij[0][0]
j = ij[1][0]
else:
loop.append(loop[0])
# Uncomment to to make the start of the loop invisible when plotting
# loop.append(loop[1])
break
loop.append(edges[i, (j + 1) % 2, :])
edges = np.delete(edges, i, axis=0)
loop_list.append(np.array(loop))
return loop_list
def plot_outlines(bool_img, ax=None, **kwargs):
if ax is None:
ax = plt.gca()
edges = get_all_edges(bool_img=bool_img)
edges = edges - 0.5 # convert indices to coordinates; TODO adjust according to image extent
outlines = close_loop_edges(edges=edges)
cl = LineCollection(outlines, **kwargs)
ax.add_collection(cl)
array = np.zeros((20, 20))
array[4:7, 3:8] = 1
array[4:7, 12:15] = 1
array[7:15, 7:15] = 1
array[12:14, 13:14] = 0
plt.figure()
plt.imshow(array, cmap='binary')
plot_outlines(array.T, lw=5, color='r')
I have a pickle file which contains 300 coordinates of my subject's location in time. There are some missing values in the middle of it for which I am using a particle filter to estimate those missing values. At the end, I am getting some predictions (not completely accurate) but in a bit drifted form.
So the position of my subject is, in fact, the position of my subject's nose. I take a total of 300 frames and each frame consists of a coordinate for nose in it. There are some frames which have the value of (0,0) meaning the values are missing. So in order to find them, I am implementing the particle filter. I am a newbie for particle filter so there are possibilities that I may have messed up the code. The results that I get, gives me the prediction for 300 frames with drifted values. You can get a clear idea form the image.
My measurement value is distance from four landmarks and I provide orientation angle to next point and distance to next point as additional measurements.
from filterpy.monte_carlo import systematic_resample
import numpy as np
import matplotlib.pyplot as plt
from numpy.linalg import norm
from numpy.random import randn
import scipy.stats
from numpy.random import uniform
import pickle
from math import *
#####################################################
def create_uniform_particles(x_range, y_range, hdg_range, N):
particles = np.empty((N, 3))
particles[:, 0] = uniform(x_range[0], x_range[1], size=N)
particles[:, 1] = uniform(y_range[0], y_range[1], size=N)
particles[:, 2] = uniform(hdg_range[0], hdg_range[1], size=N)
particles[:, 2] %= 2 * np.pi
return particles
def create_gaussian_particles(mean, std, N):
particles = np.empty((N, 3))
particles[:, 0] = mean[0] + (randn(N) * std[0])
particles[:, 1] = mean[1] + (randn(N) * std[1])
particles[:, 2] = mean[2] + (randn(N) * std[2])
particles[:, 2] %= 2 * np.pi
return particles
#####################################################
def predict(particles, u, std):
# move according to control input u (heading change, velocity)
#with noise Q (std heading change, std velocity)`
N = len(particles)
# update heading
#particles[:, 2] += u[0] + (randn(N) * std[0])
#particles[:, 2] %= 2 * np.pi
#u[0] += (randn(N) * std[0])
u[0] %= 2 * np.pi
# move in the (noisy) commanded direction
dist = (u[1]) #+ (randn(N) * std[1])
particles[:, 0] += np.cos(u[0]) * dist
particles[:, 1] += np.sin(u[0]) * dist
#####################################################
def update(particles, weights, z, R, landmarks):
for i, landmark in enumerate(landmarks):
distance = np.linalg.norm(particles[:, 0:2] - landmark, axis=1)
weights *= scipy.stats.norm(distance, R).pdf(z[i])
weights += 1.e-300 # avoid round-off to zero
weights /= sum(weights) # normalize
#####################################################
def estimate(particles, weights):
#returns mean and variance of the weighted particles
pos = particles[:, 0:2]
mean = np.average(pos, weights=weights, axis=0)
var = np.average((pos - mean)**2, weights=weights, axis=0)
return mean, var
#####################################################
def simple_resample(particles, weights):
N = len(particles)
cumulative_sum = np.cumsum(weights)
cumulative_sum[-1] = 1. # avoid round-off error
indexes = np.searchsorted(cumulative_sum, random(N))
# resample according to indexes
particles[:] = particles[indexes]
weights.fill(1.0 / N)
#####################################################
def neff(weights):
return 1. / np.sum(np.square(weights))
#####################################################
def resample_from_index(particles, weights, indexes):
particles[:] = particles[indexes]
weights[:] = weights[indexes]
weights.fill(1.0 / len(weights))
#####################################################
def read_pickle(pkl_file, f,j):
with open(pkl_file, 'rb') as res:
dets = pickle.load(res, encoding = 'latin1')
all_keyps = dets['all_keyps']
keyps_t = np.array(all_keyps[1])
keyps = np.zeros((keyps_t.shape[0], 4, 17))
for k in range(keyps.shape[0]):
if keyps_t[k]!=[]:
keyps[k] = keyps_t[k][0]
keyps = keyps[:,:2,:]
for i in range(keyps.shape[0]):
keyps[i][0] = keyps[i][0]/480*256
keyps[i][1] = keyps[i][1]/640*256
x0=keyps[f][0][j]
y0=keyps[f][1][j]
x1=keyps[f+1][0][j]
y1=keyps[f+1][1][j]
cord = np.array([x0,y0])
orientation = atan2((y1 - y0),(x1 - x0))
dist= sqrt((x1-x0) ** 2 + (y1-y0) ** 2)
u = np.array([orientation,dist])
return (cord, u)
#####################################################
def run_pf1(N, iters=298, sensor_std_err=.1,
do_plot=True, plot_particles=False,
xlim=(-256, 256), ylim=(-256, 256),
initial_x=None):
landmarks = np.array([[0, 0], [0, 256], [256,0], [256,256]])
NL = len(landmarks)
plt.figure()
# create particles and weights
if initial_x is not None:
particles = create_gaussian_particles(
mean=initial_x, std=(5, 5, np.pi/4), N=N)
else:
particles = create_uniform_particles((0,20), (0,20), (0, 6.28), N)
weights = np.ones(N) / N
if plot_particles:
alpha = .20
if N > 5000:
alpha *= np.sqrt(5000)/np.sqrt(N)
plt.scatter(particles[:, 0], particles[:, 1],
alpha=alpha, color='g')
xs = []
#robot_pos, u = read_pickle('.pkl',1,0)
for x in range(iters):
robot_pos, uv = read_pickle('.pkl',x,0)
print("orignal: ", robot_pos,)
# distance from robot to each landmark
zs = (norm(landmarks - robot_pos, axis=1) +
(randn(NL) * sensor_std_err))
# move diagonally forward to (x+1, x+1)
predict(particles, u=uv, std=(0, .0))
# incorporate measurements
update(particles, weights, z=zs, R=sensor_std_err,
landmarks=landmarks)
# resample if too few effective particles
if neff(weights) < N/2:
indexes = systematic_resample(weights)
resample_from_index(particles, weights, indexes)
assert np.allclose(weights, 1/N)
mu, var = estimate(particles, weights)
#mu +=(120,10)
xs.append(mu)
print("expected: ",mu)
if plot_particles:
plt.scatter(particles[:, 0], particles[:, 1],
color='k', marker=',', s=1)
p1 = plt.scatter(robot_pos[0], robot_pos[1], marker='+',
color='k', s=180, lw=3)
p2 = plt.scatter(mu[0], mu[1], marker='s', color='r')
print(p2)
xs = np.array(xs)
#plt.plot(xs[:, 0], xs[:, 1])
plt.legend([p1, p2], ['Actual', 'PF'], loc=4, numpoints=1)
plt.xlim(*xlim)
plt.ylim(*ylim)
print('final position error, variance:\n\t', mu - np.array([iters, iters]), var)
plt.show()
return(p2)
###############################
run_pf1(N=5000)
I expect a set of 300 coordinate values (estimated) as a result of the particle filter so I can replace my missing values in original files with this predicted ones.
I use binocular camera to reconstruct points in 3d from 2d picture,I took many pictures by binocular camera and reconstructed points(feature points have been found already),but I found that the 3d models I reconstructed are not in a same coordinate.
I don't know the extrinsic params(by the way,I wonder how to get this params,because I got the intrinsic matrix from calibration already)
so, I compute the E matrix(8 points algorithm) and assume project matrix P1 of camera1 is P[I|0] and calculate P2 by P1 and E
the last step is to calculate the points in 3d by triangulation.
Code:
def compute_normalized_image_to_image_matrix(p1, p2, compute_essential=False):
""" Computes the fundamental or essential matrix from corresponding points
using the normalized 8 point algorithm.
:input p1, p2: corresponding points with shape 3 x n
:returns: fundamental or essential matrix with shape 3 x 3
"""
n = p1.shape[1]
if p2.shape[1] != n:
raise ValueError('Number of points do not match.')
# preprocess image coordinates
p1n, T1 = scale_and_translate_points(p1)
p2n, T2 = scale_and_translate_points(p2)
# compute F or E with the coordinates
F = compute_image_to_image_matrix(p1n, p2n, compute_essential)
# reverse preprocessing of coordinates
# We know that P1' E P2 = 0
F = np.dot(T1.T, np.dot(F, T2))
return F / F[2, 2]
def compute_fundamental_normalized(p1, p2):
return compute_normalized_image_to_image_matrix(p1, p2)
def compute_essential_normalized(p1, p2):
return compute_normalized_image_to_image_matrix(p1, p2, compute_essential=True)
def scale_and_translate_points(points):
""" Scale and translate image points so that centroid of the points
are at the origin and avg distance to the origin is equal to sqrt(2).
:param points: array of homogenous point (3 x n)
:returns: array of same input shape and its normalization matrix
"""
x = points[0]
y = points[1]
center = points.mean(axis=1) # mean of each row
cx = x - center[0] # center the points
cy = y - center[1]
dist = np.sqrt(np.power(cx, 2) + np.power(cy, 2))
scale = np.sqrt(2) / dist.mean()
norm3d = np.array([
[scale, 0, -scale * center[0]],
[0, scale, -scale * center[1]],
[0, 0, 1]
])
return np.dot(norm3d, points), norm3d
def compute_P_from_fundamental(F):
""" Compute the second camera matrix (assuming P1 = [I 0])
from a fundamental matrix.
"""
e = compute_epipole(F.T) # left epipole
Te = skew(e)
return np.vstack((np.dot(Te, F.T).T, e)).T
def compute_P_from_essential(E):
""" Compute the second camera matrix (assuming P1 = [I 0])
from an essential matrix. E = [t]R
:returns: list of 4 possible camera matrices.
"""
U, S, V = np.linalg.svd(E)
# Ensure rotation matrix are right-handed with positive determinant
if np.linalg.det(np.dot(U, V)) < 0:
V = -V
# create 4 possible camera matrices (Hartley p 258)
W = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]])
P2s = [np.vstack((np.dot(U, np.dot(W, V)).T, U[:, 2])).T,
np.vstack((np.dot(U, np.dot(W, V)).T, -U[:, 2])).T,
np.vstack((np.dot(U, np.dot(W.T, V)).T, U[:, 2])).T,
np.vstack((np.dot(U, np.dot(W.T, V)).T, -U[:, 2])).T]
return P2s
def linear_triangulation(p1, p2, m1, m2):
"""
Linear triangulation (Hartley ch 12.2 pg 312) to find the 3D point X
where p1 = m1 * X and p2 = m2 * X. Solve AX = 0.
:param p1, p2: 2D points in homo. or catesian coordinates. Shape (2 x n)
:param m1, m2: Camera matrices associated with p1 and p2. Shape (3 x 4)
:returns: 4 x n homogenous 3d triangulated points
"""
num_points = p1.shape[1]
res = np.ones((4, num_points))
for i in range(num_points):
A = np.asarray([
(p1[0, i] * m1[2, :] - m1[0, :]),
(p1[1, i] * m1[2, :] - m1[1, :]),
(p2[0, i] * m2[2, :] - m2[0, :]),
(p2[1, i] * m2[2, :] - m2[1, :])
])
_, _, V = np.linalg.svd(A)
X = V[-1, :]
res[:, i] = X / X[3]
return res
so how can I solve this? I want all my reconstructed points to be in a same coordinate system,could you please tell me?thank you very much!