As far as I know, after image resize, the corresponding intrinsic parameter K also changes proportionally, but why the coordinates of the 3D reconstruction of the same point are not the same?
The following python program is a simple experiment, the original image size is , after resize it becomes , the intrinsic parameter K1 corresponds to the original image, the intrinsic parameter K2 corresponds to the resize, RT1, RT2 are the extrinsic projection matrix of the camera (should remain unchanged?,[R,T], size), without considering the effects of camera skew factor and distortions,why is there a difference in the reconstructed 3D points?
import cv2
import numpy as np
fx = 1040
fy = 1040
cx = 1920 / 2
cy = 1080 / 2
K1 = np.array([[fx, 0, cx],
[0, fy, cy],
[0, 0, 1]])
RT1 = np.array([[1, 0, 0, 4],
[0, 1, 0, 5],
[0, 0, 1, 6]]) # just random set
theta = np.pi / 6
RT2 = np.array([[np.cos(theta), -np.sin(theta), 0, 40],
[np.sin(theta), np.cos(theta), 0, 50],
[0, 0, 1, 60]]) # just random set
p1 = np.matmul(K1, RT1) # extrinsic projection matrix
p2 = np.matmul(K1, RT2) # extrinsic projection matrix
pt1 = np.array([100.0, 200.0])
pt2 = np.array([300.0, 400.0])
point3d1 = cv2.triangulatePoints(p1, p2, pt1, pt2)
# Remember to divide out the 4th row. Make it homogeneous
point3d1 = point3d1 / point3d1[3]
print(point3d1)
[[-260.07160113]
[ -27.39546108]
[ 273.95189881]
[ 1. ]]
then resize image to test recontruct 3D point, see if it is numerical equal.
rx = 640.0 / 1920.0
ry = 480.0 / 1080.0
fx = fx * rx
fy = fy * ry
cx = cx * rx
cy = cy * ry
K2 = np.array([[fx, 0, cx],
[0, fy, cy],
[0, 0, 1]])
p1 = np.matmul(K2, RT1)
p2 = np.matmul(K2, RT2)
pt1 = np.array([pt1[0] * rx, pt1[1] * ry])
pt2 = np.array([pt2[0] * rx, pt2[1] * ry])
point3d2 = cv2.triangulatePoints(p1, p2, pt1, pt2)
# Remember to divide out the 4th row. Make it homogeneous
point3d2 = point3d2 / point3d2[3]
print(point3d2)
[[-193.03965985]
[ -26.72133393]
[ 189.12512305]
[ 1. ]]
you see, point3d1 and point3d2 is not same,why?
After careful consideration, I was lucky to get a more plausible explanation, which I now state as follows to help others.
In a short conclusion:
Image scaling must specify a uniform (fx=fy) scaling factor in order to derive the correct intrinsic parameter K, otherwise inconsistencies in the x,y axis focal lengths with respect to the original image directly lead to deviations in the calculated 3D points!
Returning to the problem at the beginning, the given image size is 1080×1920, and its focal length is 1040 pixels, i.e. fx=fy=1040, because by definition fx=f/dx,fy=f/dy, where dx, dy are the number of pixels per unit length, and f is the actual physical size of the focal length; thus the a priori dx=dy can be introduced, which is constant This "convention" should also be followed for later image scaling.
Imagine if the scaled image fx,fy were obtained in different proportions, dx,dy would not be the same, causing distortion of the image, and in addition, according to the external projection matrix P = K*[R,t], fx,fy in K would vary disproportionately leading to a deviation in the calculated P!
BTW, Similarly, I put the reference answer to the experiment done by matlab at this link.
I have two copies of the same molecule as .xyz file. This means that each atom is has X, Y and Z coordinates. However, you can rotate the molecule and obtain different coordinates for each atom, although the relative positions are the same and the molecule remains the same. I want to align the two molecules using three atoms as reference points. However, I am struggling to completely align the two molecules.
Firstly, I align both molecules by translation for a single atom. Then, I am doing two subsequent rotation using rotation matrices as explained elsewhere. For some reason, I need to take the negative of the cross product of both vectors and use a sinus instead of a cosinus to get both structures to be perfectly aligned (I discovered this after a lot of trial and error).
For the second rotation, I project both vectors I want to align on a plane defined by the rotation vector. This is necessary because I don't want to rotate along the cross product of the two vectors to align, since that would disalign the rest of the molecule. Instead, I rotate along the two already aligned vectors. The project allows me to find the angle in the plane between the two vectors, and thus the rotation necessary.
However, this code does not properly align the two molecules.
"group1[0]" contains the XYZ coordinates of the three atoms to align in a list. Likewise for "group2[0]" and the structure 2.
#Point 1: align the functional groups to the origin
O1 = np.array(coords1[group1[0][0]])
O2 = np.array(coords2[group2[0][0]])
mat_2 = np.zeros((len(atoms2), 3))
for ind, c in enumerate(coords1):
coords1[ind] = np.array(c) - O1
for ind, c in enumerate(coords2):
coords2[ind] = np.array(c) - O2
mat_2[ind] = coords2[ind]
#Point 2: align according to a first vector
v1 = np.array(coords1[group1[0][1]])#Since atom 1 is the origin, the coordinates is the vector already
v2 = np.array(coords2[group2[0][1]])#Since atom 1 is the origin, the coordinates is the vector already
v1 = v1/np.linalg.norm(v1)
v2 = v2/np.linalg.norm(v2)
#Let v be the axis of rotation
v = -np.cross(v1, v2)#why do I need a minus here?
if np.linalg.norm(v) != 0:
a = np.arccos(np.dot(v1, v2)/(np.linalg.norm(v1)*np.linalg.norm(v2)))
#c = np.dot(v1, v2)*np.cos(a)
c = np.dot(v1, v2)*np.sin(a)#The internet says cos, but this works perfectly
vx = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
rot_mat = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) + vx + vx.dot(vx)*(1-c)/(1-c**2)
mat_2 = np.array(mat_2)
R_mat_rot = np.matmul(rot_mat, mat_2.T).T
else:
exit(0)
coords3 = R_mat_rot.copy()
#I get exactly what I want up until here
#Point 3: Rotate along atom2-atom1 (v1) to align the third atom
v = -v1.copy()
v2 = np.array(coords3[group2[0][2]]) - np.array(coords3[group2[0][0]]) #Since atom 1 is the origin, the coordinates is the vector already
v2 = v2/np.linalg.norm(v2)
v1 = np.array(coords1[group1[0][2]]) - np.array(coords1[group1[0][0]]) #Since atom 1 is the origin, the coordinates is the vector already
v1 = v1/np.linalg.norm(v1)
if np.linalg.norm(v) != 0:
#consider v to be the vector normal to a plane
#we want the projection of v1 and v2 unto that plane
vp1 = np.cross(v, np.cross(v1, v)) - np.array(coords1[group1[0][0]])
vp1 = vp1/np.linalg.norm(vp1)
vp2 = np.cross(v, np.cross(v2, v)) - np.array(coords3[group2[0][0]])
vp2 = vp2/np.linalg.norm(vp2)
#we find the angle between those vectors on the plane
a = np.arccos(np.dot(vp1, vp2))/(np.linalg.norm(vp1)*np.linalg.norm(vp2))
#rotation of that amount
c = np.dot(v1, v2)*np.cos(a)
vx = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
rot_mat = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) + vx + np.dot(vx, vx)*(1-c)/(1-c**2)
R_mat_rot = np.matmul(rot_mat, coords3.T).T
coords4 = R_mat_rot.copy()#Final coordinates
I have built a simple robotic arm using 3 RC Servos and an Arduino.
I just want to play around with it and learn something about robotics.
Currently, I am trying to compute the position of the tip of the robotic arm using the three angular positions of the servos.
"Forward kinematics" I think is the technical term for this.
Btw the tip of the arm is a pen, I thought I might try to draw something with it later on.
In the movement range of the arm I set up a Cartesian coordinate system and recorded 24 (angle => position) samples.
pastebin.com/ESqWzJJB
Now, I am trying to model this data, but I am a bit out of my depth here.
Here is my approach so far:
I use the Denavit–Hartenberg equations found on Wikipedia en.wikipedia.org/wiki/Denavit–Hartenberg_parameters.
I then try to determine the parameters using least squares optimization.
minimize(sum(norm(f(x,P)-y)^2))
I also added linear terms to the input and output of the model to compensate for possible distortions (e.g. phase-shift in the servo angle):
y = f(ax+b)*c+d
My Python code: pastebin.com/gQF72mQn
from numpy import *
from scipy.optimize import minimize
# Denavit-Hartenberg Matrix as found on Wikipedia "Denavit-Hartenberg parameters"
def DenHarMat(theta, alpha, a, d):
cos_theta = cos(theta)
sin_theta = sin(theta)
cos_alpha = cos(alpha)
sin_alpha = sin(alpha)
return array([
[cos_theta, -sin_theta*cos_alpha, sin_theta*sin_alpha, a*cos_theta],
[sin_theta, cos_theta*cos_alpha, -cos_theta*sin_alpha, a*sin_theta],
[0, sin_alpha, cos_alpha, d],
[0, 0, 0, 1],
])
def model_function(parameters, x):
# split parameter vector
scale_input, parameters = split(parameters,[3])
translate_input, parameters = split(parameters,[3])
scale_output, parameters = split(parameters,[3])
translate_output, parameters = split(parameters,[3])
p_T1, parameters = split(parameters,[3])
p_T2, parameters = split(parameters,[3])
p_T3, parameters = split(parameters,[3])
# compute linear input distortions
theta = x * scale_input + translate_input
# load Denavit-Hartenberg Matricies
T1 = DenHarMat(theta[0], p_T1[0], p_T1[1], p_T1[2])
T2 = DenHarMat(theta[1], p_T2[0], p_T2[1], p_T2[2])
T3 = DenHarMat(theta[2], p_T3[0], p_T3[1], p_T3[2])
# compute joint transformations
# y = T1 * T2 * T3 * [0 0 0 1]
y = dot(T1,dot(T2,dot(T3,array([0,0,0,1]))))
# compute linear output distortions
return y[0:3] * scale_output + translate_output
# least squares cost function
def cost_function(parameters, X, Y):
return sum(sum(square(model_function(parameters, X[i]) - Y[i])) for i in range(X.shape[0])) / X.shape[0]
# ========== main script start ===========
# load data
data = genfromtxt('data.txt', delimiter=',', dtype='float32')
X = data[:,0:3]
Y = data[:,3:6]
cost = 9999999
#try:
# parameters = genfromtxt('parameters.txt', delimiter=',', dtype='float32')
# cost = cost_function(parameters, X, Y)
#except IOError:
# pass
# random init
for i in range(100):
tmpParams = (random.rand(7*3)*2-1)*8
tmpCost = cost_function(tmpParams, X, Y)
if tmpCost < cost:
cost = tmpCost
parameters = tmpParams
print('Random Cost: ' + str(cost))
savetxt('parameters.txt', parameters, delimiter=',')
# optimization
continueOptimization = True
while continueOptimization:
res = minimize(cost_function, parameters, args=(X,Y), method='nelder-mead', options={'maxiter':100,'xtol': 1e-5})
parameters = res.x
print(res.fun)
savetxt('parameters.txt', parameters, delimiter=',')
continueOptimization = not res.success
print(res)
But it just won't work, none of my attempts have converged on a good solution.
I also tried a simple 3x4 matrix multiplication, which does not make much sense as a model, but oddly it didn't do worse than the more sophisticated model above.
I hope there is someone out there who can help.
If I understood you correctly, you are trying to solve the inverse kinematics (IK) of your robot arm. Forward kinematics (FK) is about figuring out where your end-effector is located given the joint angles. You want to find the angles that makes the end-effector reach the desired position.
In order to solve the IK problem you have to figure out the forward-kinematics of your arm.
If you are unsure about your current FK, you could use the following script to get the symbolic FK matrices for each joint (including end-effector). It also generates the Jacobian.
import numpy as np
from sympy import *
def pos(matrix):
list = [0,0,0]
list[0] = matrix[0,3]
list[1] = matrix[1,3]
list[2] = matrix[2,3]
return np.array(list).astype(float).tolist()
class KinematicChain:
def __init__(self):
self.i = 1
self.syms = []
self.types = []
self.matrices = []
self.fk = []
def add(self, type, relPos):
"""
Parameters:
type - the type of joint
relpos - the position of the joint relative to the previos one
"""
mat = self.transMatrix(type, relPos);
self.matrices.append(mat)
self.types.append(type)
if len(self.fk) == 0:
self.fk.append(eye(4)*mat)
else:
self.fk.append(simplify(self.fk[-1]*mat))
def jacobian(self):
fk = self.fk[-1]
px = fk[0,3]
py = fk[1,3]
pz = fk[2,3]
f = Matrix([px, py, pz])
if (len(self.syms) < 1):
return eye(4)
else:
x = Matrix(self.syms)
ans = f.jacobian(x)
return ans
def transMatrix(self, type, p):
if (type != "FIXED"):
s1 = "a" + str(self.i)
self.i += 1
a = symbols(s1)
self.syms.append(a)
if (type == "FIXED"):
return Matrix([
[1, 0, 0, p[0]],
[0, 1, 0, p[1]],
[0, 0, 1, p[2]],
[0, 0, 0, 1]])
elif (type == "RX"):
return Matrix([
[1, 0, 0, p[0]],
[0, cos(a), -sin(a), p[1]],
[0, sin(a), cos(a), p[2]],
[0, 0, 0, 1]])
elif (type == "RY"):
return Matrix([
[cos(a), 0, sin(a), p[0]],
[0, 1, 0, p[1]],
[-sin(a), 0, cos(a), p[2]],
[0, 0, 0, 1]])
elif (type == "RZ"):
return Matrix([
[cos(a), -sin(a), 0, p[0]],
[sin(a), cos(a), 0, p[1]],
[0, 0, 1, p[2]],
[0, 0, 0, 1]])
elif (type == "PX"):
return Matrix([
[1, 0, 0, p[0] + a],
[0, 1, 0, p[1]],
[0, 0, 1, p[2]],
[0, 0, 0, 1]])
elif (type == "PY"):
return Matrix([
[1, 0, 0, p[0]],
[0, 1, 0, p[1] + a],
[0, 0, 1, p[2]],
[0, 0, 0, 1]])
elif (type == "PZ"):
return Matrix([
[1, 0, 0, p[0]],
[0, 1, 0, p[1]],
[0, 0, 1, p[2] + a],
[0, 0, 0, 1]])
else:
return eye(4)
There are many ways of solving the IK. A good one is the Damped Least Squared method.
See: http://math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/iksurvey.pdf
A simpler method is Cyclic Coordinate Decent which is quite manageable to get working on a arduino with limited matrix support. See: http://www.cs.cmu.edu/~15464-s13/assignments/assignment2/jlander_gamedev_nov98.pdf
I think what you are trying to do is some kind of "kinematics calibration": identifying the robot parameters from a set of measurement data. There are numerous classic text books discussing this topic if you really want to dig deeper, for example [Mooring et al.] "Fundamentals of manipulator calibration".
Back to your question, a lot of things can cause your parameter identification to fail converging, so mind you this is not a cookbook answer. ;)
One possible case is you have two (or more) joints with parallel axes. It is quite common to have this kind of configuration in simpler robots, for instance in SCARA, or PUMA-like mechanisms. In this case, with DH convention there are infinitely many way to choose the axis lines.
There are different approaches to cope with this, but YMMV. One thing that you can try is to use Hayati-modified DH model. This model adds one more parameter "beta" to the basic DH, to cope with singularity in parallel-axis case.
Or you can try creating your own "custom" transformation matrices to model your mechanism. For example, you can use roll-pitch-yaw (or Euler angles) to represent the rotation between joint axes, then add one length parameter to reach the next joint, etc.
Another thing that caught my attention is the _scale_output_. I think this means you can have multiple "arm length" solutions for a given data set. As an illustration, both [scale_output=1, arm_length=100] and [scale_output=100, arm_length=1] will give the same position with same joint angles. Try removing the scale_output from the model, and see if that helps.
Also you might want to try other optimization/minimization routines. I successfully used scipy.optimize.leastsq() for kinematics calibration in the past.
Hope this helps!
Seeing that your goal is to learn more about robotics, building strong fundamentals first will greatly assist you in the long run. You'll most likely want to dive into the world of transformation matrices first so you'll have something to build off of when you get to more complex topics like DH-tables and inverse kinematics.
Here are some videos that may help:
https://www.youtube.com/watch?v=xYQpeKYCfGs&list=PLJtm2YNbaY4_rQApwht0ia5r_sx3vaSxv
What is the easiest/fastest way to take a weighted sum of values in a numpy array?
Example: Solving the heat equation with the Euler method
length_l=10
time_l=10
u=zeros((length_l,length_l))# (x,y)
u[:, 0]=1
u[:,-1]=1
print(u)
def dStep(ALPHA=0.1):
for position,value in ndenumerate(u):
D2u= (u[position+(1,0)]-2*value+u[position+(-1, 0)])/(1**2) \
+(u[position+(0,1)]-2*value+u[position+( 0,-1)])/(1**2)
value+=ALPHA*D2u()
while True:
dStep()
print(u)
D2u should be the second central difference in two dimensions. This would work if I could add indexes like (1,4)+(1,3)=(2,7). Unfortunately, python adds them as (1,4)+(1,3)=(1,4,1,3).
Note that computing D2u is equivalent to taking a dot product with this kernel centered around the current position:
0, 1, 0
1,-4, 1
0, 1, 0
Can this be vectorised as a dot product?
I think you want something like:
import numpy as np
from scipy.ndimage import convolve
length_l = 10
time_l = 10
u = np.zeros((length_l, length_l))# (x,y)
u[:, 0] = 1
u[:, -1] = 1
alpha = .1
weights = np.array([[ 0, 1, 0],
[ 1, -4, 1],
[ 0, 1, 0]])
for i in range(5):
u += alpha * convolve(u, weights)
print(u)
You could reduce down a bit by doing:
weights = alpha * weights
weights[1, 1] = weights[1, 1] + 1
for i in range(5):
u = convolve(u, weights)
print(u)