scipy.optimize.curve_fit raises a runtime error

This is the first time I'm using curve_fit and I haven't found examples that would match my problem. My question is, am I using curve_fit correctly data-format-wise ? If yes then my problem is mathematical and I'll have to correct it (I haven't found any mistakes yet...). Here's my code :

import numpy as np
import math as m
import scipy.optimize as sci

def f4(X,Tx,Ty,Tz,i,j,k):
    res=[]
#X looks like [x1,y1,z1,x2,y2,z2....]
    for n in range(np.shape(X)[0]/3):
        xr=X[n*3]+Tx+m.cos(j)*m.cos(k)*X[n*3]-m.cos(j)*m.sin(k)*X[n*3+1]+m.sin(j)*X[n*3+2]
        yr=X[n*3+1]+Ty+(m.sin(i)*m.sin(j)*m.cos(k)+m.cos(i)*m.sin(k))*X[n*3]+(-m.sin(i)*m.sin(j)*m.sin(k)+m.cos(i)*m.cos(k))*X[n*3+1]-m.sin(i)*m.cos(j)*X[n*3+2]
        zr=X[n*3+2]+Tz+(-m.cos(i)*m.sin(j)*m.cos(k)+m.sin(i)*m.sin(k))*X[n*3]+(m.cos(i)*m.sin(j)*m.sin(k)+m.sin(i)*m.cos(k))*X[n*3+1]+m.cos(i)*m.cos(j)*X[n*3+2]
        res.append(xr)
        res.append(yr)
        res.append(zr)
    return res

xdata2=[998.362,5000.052,99.4,997.862,5000.052,99.4,998.362,5000.052,98.9,997.862,5000.052,98.9]
ydata2=[999.46555,4999.801,99.4,999.112,5000.15455,99.4,999.46555,4999.801,98.9,999.112,5000.15455,98.9]
p0=[1,0,0,0,0,0.8]

popt,pcov=sci.curve_fit(f4,xdata2,ydata2,p0)

it raises : RuntimeError: Optimal parameters not found: Number of calls to function has reached maxfev = 1400.

According to my calculation, p0 is pretty close to the solution. Is my code okay or should I keep looking for a mathematical mistake ?

if anybody's curious I'm trying to find the 3 translations and the 3 rotations I must apply to a set of points Xp in order to obtain a set of points Xr.

Any piece of advice is appreciated

Edit 1/04 :

I tried unutbu's way I changed my function :

def f2(X,Tx,Ty,Tz,i,j,k):
    res=[]
    for n in range(np.shape(X)[0]):
        xr=X[n][0]+Tx+m.cos(j)*m.cos(k)*X[n][0]-m.cos(j)*m.sin(k)*X[n][1]+m.sin(j)*X[n][2]
    yr=X[n][1]+Ty+(m.sin(i)*m.sin(j)*m.cos(k)+m.cos(i)*m.sin(k))*X[n][0]+(-m.sin(i)*m.sin(j)*m.sin(k)+m.cos(i)*m.cos(k))*X[n][1]-m.sin(i)*m.cos(j)*X[n][2]
    zr=X[n][2]+Tz+(-m.cos(i)*m.sin(j)*m.cos(k)+m.sin(i)*m.sin(k))*X[n][0]+(m.cos(i)*m.sin(j)*m.sin(k)+m.sin(i)*m.cos(k))*X[n][1]+m.cos(i)*m.cos(j)*X[n][2]
    aux=[xr,yr,zr]
    res.append(aux)
res=np.array(res)
return res

I added two points to my arrays :

xdata3=np.array([[998.362,5000.052,99.4],[997.862,5000.052,99.4],[998.362,5000.052,98.9],[997.862,5000.052,98.9],[999.112,4999.801,98.9],[999.112,4999.801,99.4]])
ydata3=np.array([[999.46555,4999.801,99.4],[999.112,5000.15455,99.4],[999.46555,4999.801,98.9],[999.112,5000.15455,98.9],[1000.112,4999.801,98.9],[1000.112,4999.801,99.4]])

I tried out the function :

test=f2(xdata3,1,0.00001,0.00001,0.00001,0.00001,0.8)

In [17]:test
Out[17]:
array([[-1891.88925911,  9199.80185261,   198.87092002],
   [-1892.73761247,  9199.44317456,   198.87091992],
   [-1891.88926411,  9199.80185761,   197.87092002],
   [-1892.73761747,  9199.44317956,   197.87091992],
   [-1890.4366777 ,  9199.91400129,   197.87091663],
   [-1890.4366727 ,  9199.91399629,   198.87091663]])

Changed p0 too:

p0=[1,0.00001,0.00001,0.00001,0.00001,0.8]

Then i tried the curve_fit :

test=f2(xdata3,1,0.00001,0.00001,0.00001,0.00001,0.8)

And I've got a different error :

error: Result from function call is not a proper array of floats.

Not sure what happened since my test variable seems fine.

Solution

I think the problem lies solely in your choice of model function. The data simply doesn't fit that model.

If you want to fit the data using a translation and a rotation, perhaps what you are looking for is more like f4 (below), which

finds the centroid of the object, Xbar. X-Xbar moves the object back to a position closer to the origin.
rotates X-Xbar about the origin (using np.dot(R, X-Xbar)), and then
translates by T + Xbar.

(Note that, unlike the original f4, it does not also translate by X.)

import numpy as np
import scipy.optimize as optimize
from math import cos, sin

def f4(X,Tx,Ty,Tz,i,j,k):
    T = np.array([Tx, Ty, Tz])[:, np.newaxis]
    X = X.reshape(-1,3).T
    Xbar = X.mean(axis=1)[:, np.newaxis]
    X = X - Xbar
    R = np.array([
        (cos(j)*cos(k), -cos(j)*sin(k), sin(j)),

        (sin(i)*sin(j)*cos(k)+cos(i)*sin(k), 
         -sin(i)*sin(j)*sin(k)+cos(i)*cos(k), 
         -sin(i)*cos(j)),

        (-cos(i)*sin(j)*cos(k)+sin(i)*sin(k), 
         cos(i)*sin(j)*sin(k)+sin(i)*cos(k), 
         cos(i)*cos(j))])
    result = np.dot(R, X) + T + Xbar
    return result.T.ravel()

p0=[1,0,0,0,0,0.8]

xdata2 = np.array([998.362,5000.052,99.4,997.862,5000.052,99.4,998.362,5000.052,98.9,997.862,5000.052,98.9])
ydata2 = [999.46555,4999.801,99.4,999.112,5000.15455,99.4,999.46555,4999.801,98.9,999.112,5000.15455,98.9]

popt, pcov = optimize.curve_fit(f4, xdata2, ydata2, p0)
print(popt)

yields

[  1.17677500e+00  -7.42250000e-02  -4.02305065e-37  -1.28557241e-20
  -1.61334110e-20  -7.85398164e-01]