Related
Like the question says, I am looking for a complete, minimal, working example of the structure from motion (aka 3d reconstruction) pipeline.
Right away let me say I do not have the camera parameters. I do not know focal length or camera intrinsics. So right away, 90% of the examples/tutorials out there are not valid.
There are many questions on this topic but the code is just in snippets, and not for the complete SfM process. Many instructions are contradictory, or are just guessing, and open-source external libraries are hard to follow.
So I am looking for a short, complete, minimal, working example. Most importantly is the working requirement, since so much code out there produces bad results.
I have made a stab at it with the code below. I use synthetic data of matching pairs so there is no noise or bad correspondence issues to work around. The goal is to reconstruct a cube (8 3d points) from 2 views, each with 8 2d points. However, the final results are awful. There is no semblance of a cube shape. (I have tried normalizing and centering the data, that is not the issue).
Anyone who can provide a better minimal working example, or point out what is wrong with my attempt, is appreciated.
import cv2
import numpy as np
import scipy.linalg
def combineTR(T,R): #turn a translation vector and a rotation matrix into one 3x4 projection matrix
T4 = np.eye(4)
T4[:3, 3] = T # make it 4x4 so we can dot product it
R4 = np.eye(4)
R4[:3, :3] = R
P = np.dot(T4, R4) # combine rotation and translation into one matrix
P = P[:3, :] # cut off bottom row
return P
####################################################################
# # ground truth
# Wpts = np.array([[1, 1, 1], # A Cube in world points
# [1, 2, 1],
# [2, 1, 1],
# [2, 2, 1],
# [1, 1, 2],
# [1, 2, 2],
# [2, 1, 2],
# [2, 2, 2]])
views = np.array(
[[[ 0.211, 0.392],
[ 0.179, 0.429],
[ 0.421, 0.392],
[ 0.358, 0.429],
[ 0.189, 0.193],
[ 0.163, 0.254],
[ 0.378, 0.193],
[ 0.326, 0.254]],
[[ 0.392, 0.211],
[ 0.392, 0.421],
[ 0.429, 0.179],
[ 0.429, 0.358],
[ 0.193, 0.189],
[ 0.193, 0.378],
[ 0.254, 0.163],
[ 0.254, 0.326]]])
F = cv2.findFundamentalMat(views[0], views[1],cv2.FM_8POINT)[0]
# hartley and zimmermans method for finding P
e2 = scipy.linalg.null_space(F.T) #epipole of second image
C2R = np.cross(e2.T, F) #camera 2 rotation
C2T = e2.T[0]
P = combineTR(C2T, C2R) #projection matrix for camera 2
R = np.eye(3) # rotation matrix for camera 1
T = [0, 0, 0] # translation
P0 = combineTR(T,R)
tpts = cv2.triangulatePoints(P0,P,views[0].T,views[1].T) #triangulated point
tpts /= tpts[-1] #divide by last row and scale it
tpts *= -100
print(tpts)
Ground truth:
My results:
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 the following problem. I have 3D point coordinates and I want to conenct them in an array or "draw a line" like you can do it in 2D with skimage(http://scikit-image.org/docs/0.13.x/api/skimage.draw.html#skimage.draw.line). The optimal case would be if i could directly draw a cylinder with a radius in an array and give different radius different values like rings arround the line.(wrinkles could be a problem here). There have been approaches to do that but they are not the right thing i think like here:
"Draw" a 3d line into an array
With the two approaches of #Paul Panzer (Fastest way to get all the points between two (X,Y) coordinates in python) you get all coordinates between two 2D Points, but how would it look like in 3D, esspecially the second approach which is faster?:
import numpy as np
from timeit import timeit
def connect(ends):
d0, d1 = np.abs(np.diff(ends, axis=0))[0]
if d0 > d1:
return np.c_[np.linspace(ends[0, 0], ends[1, 0], d0+1, dtype=np.int32),
np.linspace(ends[0, 1]+0.5, ends[1, 1]+0.5, d0+1, dtype=np.int32)]
else:
return np.c_[np.linspace(ends[0, 0]+0.5, ends[1, 0]+0.5, d1+1, dtype=np.int32),
np.linspace(ends[0, 1], ends[1, 1], d1+1, dtype=np.int32)]
def connect2(ends):
d0, d1 = np.diff(ends, axis=0)[0]
if np.abs(d0) > np.abs(d1):
return np.c_[np.arange(ends[0, 0], ends[1,0] + np.sign(d0), np.sign(d0), dtype=np.int32),
np.arange(ends[0, 1] * np.abs(d0) + np.abs(d0)//2,
ends[0, 1] * np.abs(d0) + np.abs(d0)//2 + (np.abs(d0)+1) * d1, d1, dtype=np.int32) // np.abs(d0)]
else:
return np.c_[np.arange(ends[0, 0] * np.abs(d1) + np.abs(d1)//2,
ends[0, 0] * np.abs(d1) + np.abs(d1)//2 + (np.abs(d1)+1) * d0, d0, dtype=np.int32) // np.abs(d1),
np.arange(ends[0, 1], ends[1,1] + np.sign(d1), np.sign(d1), dtype=np.int32)]
ends = np.array([[ 1520, -1140],
[ 1412, -973]])
Since you tagged the question 'vtk', I assume you can use it in your code. In that case, I think vtkTubeFilter does exactly what you are looking for - you create a vtkPolyData with all your line segments (you have your endpoints for that) and for each segment, the filter generates a cylinder. You can also specify different radius for each segment as you wanted: call SetVaryRadiusToVaryRadiusByScalar() on the filter to turn it on and provide the array with radii as scalar data.
You can find a code example that does all that (and more) here: https://www.vtk.org/Wiki/VTK/Examples/Cxx/VisualizationAlgorithms/TubesWithVaryingRadiusAndColors
I've put together this Forward Kinematics function for Baxter arm robot based on its hardware specs and the following joints axis:
The joint positions for the following forward kinematics are not matching the corresponding Cartesian coordinates, what am I doing wrong here?
def FK_function_2(joints):
def yaw(theta): #(rotation around z)
y = np.array([[np.cos(theta), -np.sin(theta), 0],
[np.sin(theta), np.cos(theta), 0],
[0, 0, 1] ])
return y
R01 = yaw(joints[0]).dot(np.array([[-1, 0, 0],
[0, 0, 1],
[0, 1, 0]]))
R12 = yaw(joints[1]).dot(np.array([[0, 0, -1],
[-1, 0, 0],
[0, 1, 0]]))
R23 = yaw(joints[2]).dot(np.array([[-1, 0, 0],
[0, 0, 1],
[0, 1, 0]]))
R34 = yaw(joints[3]).dot(np.array([[-1, 0, 0],
[0, 0, 1],
[0, 1, 0]]))
R45 = yaw(joints[4]).dot(np.array([[-1, 0, 0],
[0, 0, 1],
[0, 1, 0]]))
R56 = yaw(joints[5]).dot(np.array([[-1, 0, 0],
[0, 0, 1],
[0, 1, 0]]))
R67 = yaw(joints[6]).dot(np.array([[1, 0, 0],
[0, 1, 0],
[0, 0, 1]]))
d = np.array([0.27035, 0, 0.36435, 0, 0.37429, 0, 0.229525])
a = np.array([0.069, 0, 0.069, 0, 0.010, 0, 0])
l1 = np.array([a[0]*np.cos(joints[0]), a[0]*np.sin(joints[0]), d[0]]);
l2 = np.array([a[1]*np.cos(joints[1]), a[1]*np.sin(joints[1]), d[1]]);
l3 = np.array([a[2]*np.cos(joints[2]), a[2]*np.sin(joints[2]), d[2]]);
l4 = np.array([a[3]*np.cos(joints[3]), a[3]*np.sin(joints[3]), d[3]]);
l5 = np.array([a[4]*np.cos(joints[4]), a[4]*np.sin(joints[4]), d[4]]);
l6 = np.array([a[5]*np.cos(joints[5]), a[5]*np.sin(joints[5]), d[5]]);
l7 = np.array([a[6]*np.cos(joints[6]), a[6]*np.sin(joints[6]), d[6]]);
unit = np.array([0, 0, 0, 1])
H0 = np.concatenate((np.concatenate((R01, l1.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0)
H1 = np.concatenate((np.concatenate((R12, l2.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0)
H2 = np.concatenate((np.concatenate((R23, l3.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0)
H3 = np.concatenate((np.concatenate((R34, l4.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0)
H4 = np.concatenate((np.concatenate((R45, l5.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0)
H5 = np.concatenate((np.concatenate((R56, l6.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0)
H6 = np.concatenate((np.concatenate((R67, l7.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0)
T = H0.dot(H1).dot(H2).dot(H3).dot(H4).dot(H5).dot(H6)
return T[0:3, 3]
Ok, so I have been looking at this and checked your code. The code is good and works with your defined kinematic chain with transformations from the base to the end of the robotic arm.
(H0 * H1 * H2 * H3 * H4 * H5 * H6) is the correct kinematic chain where each represents a transformation from one joint to the next in the chain starting at the base of the arm.
The problem is your transformations are wrong. Your representation of H0 through H6 is not right and it is the numbers in these matrices that cause your transformations to not match the real transformations that take place. You need to from up the correct transformations from the base all the way to the end of the arm. Other than that, your approach is correct.
It looks like you are using normal DH parameters for your transformation matrices. Your values for a and d (and alpha which isn't shown in your code) are off and causing the transformations to be expressed incorrectly. The DH parameters are seen in https://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters.
I found an exact guide for Baxter's forward kinematics to help after going through the modified DH table to set up the transformations. I would look at the modified DH parameters at the end of the wiki article above since the guide uses that.
Baxter Forward Kinematic Guide: https://www.ohio.edu/mechanical-faculty/williams/html/pdf/BaxterKinematics.pdf
In this paper, the author, Robert Williams, sets up the DH parameters for the Baxter robotic arm and gets values different than what you have (I know you are using the normal DH parameters, but I would look at using the modified ones). His table is:
With lengths of:
And using the modified DH matrix:
Now you can calculate matrices H0 through H6 and if you want you can also add the end effector geometry if you have that for an additional H7. Once you multiple them all together you should get the proper forward kinematic transformation (see the paper for additional resource). Both the left and right arms have the same kinematics.
When you multiply it all together then you get the expressions for the coordinates of x7, y7, and z7 from the base of the arm that are functions of the rotations of the joints and the geometry of the robot arm. See the paper on page 17 for the expressions for x7,y7, and z7. Also see page 14 for the individual transformations.
Also don't forget to express the angles in radians since your code uses regular trig functions.
One last update:
I just remembered that it is easier for me to think of the intermediate translation and rotational steps one-by-one (instead of jumping straight to the DH matrix). The two approaches will be equivalent, but I like to think of each individual step that it takes to get from one rotation frame to the next.
For this you can use these building blocks.
Pure Translation:
[1 0 0 u;
0 1 0 v;
0 0 1 w;
0 0 0 1]
Where u is the distance from the previous frame to the new frame measured from the previous x frame axis.
Where v is the distance from the previous frame to the new frame measured from the previous y frame axis.
Where w is the distance from the previous frame to the new frame measured from the previous z frame axis.
Rotation about the z-axis by arbitrary theta:
This represent the robot joint rotation to an arbitrary theta.
[cos(theta) -sin(theta) 0 0;
sin(theta) cos(theta) 0 0;
0 0 1 0;
0 0 0 1]
Combination of rotations around intermediate frames to get to final frame position: (these angles will usually be in increments of pi/2 or pi to be able to get to the final orientation)
Can use a rotation about the intermediate x axis, y axis, or z axis shown below.
(Rotation about x axis by alpha)
R_x(alpha) = [1 0 0 0;
0 cos(alpha) -sin(alpha) 0;
0 sin(alpha) cos(alpha) 0;
0 0 0 1];
(Rotation about y axis by beta)
R_y(beta) = [ cos(beta) 0 sin(beta) 0;
0 1 0 0;
-sin(beta) 0 cos(beta) 0;
0 0 0 1];
(Rotation about z axis by gamma):
[cos(gamma) -sin(gamma) 0 0;
sin(gamma) cos(gamma) 0 0;
0 0 1 0;
0 0 0 1]
So with these building blocks you can build the sequence of steps to go from one frame to another (essentially any H matrix can be decomposed into these steps). The chain would be something like:
[H](transformation from previous frame to the next frame) = [Pure Translation from previous joint to new joint expressed in the previous joint's frame] * [Rotation about previous frame's z-axis by theta (for the joint) (since the joint has many positions, theta is left as symbolic)] * [All other intermediate rotations to arrive at the new joint frame orientation expressed as rotations about intermediate axis frames]
That is essentially what the DH parameters helps you do, but I like to think of the individual steps to get from one frame to the next instead of jumping there with the DH parameters.
With the corrected H0 through H6 transformations, your approach is correct. Just change the definitions of H0 through H6 in your code.
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