Wednesday, 10 February 2021

Scipy optimize unable to find the correct results

I am trying to use scipy.optimize.minimize to fit parameters for a multivariate function, however, regardless of how many noise free data points I am providing to the optimizer, the optimizer could not converge to a correct (or close) answer.

I wonder if there is a mistake in the way I am using the optimizer but I have been scratching my head to find the mistake. I would appreciate any advice or guesses, thanks!

import numpy as np
from scipy.optimize import minimize
import math

def get_transform(ai,aj,ak,x,y,z):

    i,j,k = 0, 1, 2
    si, sj, sk = math.sin(ai), math.sin(aj), math.sin(ak)
    ci, cj, ck = math.cos(ai), math.cos(aj), math.cos(ak)
    cc, cs = ci*ck, ci*sk
    sc, ss = si*ck, si*sk
    M = np.identity(4)
    M[i, i] = cj*ck
    M[i, j] = sj*sc-cs
    M[i, k] = sj*cc+ss
    M[j, i] = cj*sk
    M[j, j] = sj*ss+cc
    M[j, k] = sj*cs-sc
    M[k, i] = -sj
    M[k, j] = cj*si
    M[k, k] = cj*ci
    M[0, 3] = x
    M[1, 3] = y
    M[2, 3] = z
    
    return M

def camera_intrinsic(fx, ppx, fy, ppy):
    K = np.zeros((3, 3), dtype='float64')
    K[0, 0], K[0, 2] = fx, ppx
    K[1, 1], K[1, 2] = fy, ppy

    K[2, 2] = 1

    return K

def apply_transform(p, matrix):
    rotation = matrix[0:3,0:3]
  
    T = np.array([matrix[0][3],matrix[1][3],matrix[2][3]])
    transformed = (np.dot(rotation, p.T).T)+T
    return transformed

def project(points_3D,internal_calibration):
    points_3D = points_3D.T
    projections_2d = np.zeros((2, points_3D.shape[1]), dtype='float32')
    camera_projection = (internal_calibration).dot(points_3D)
    projections_2d[0, :] = camera_projection[0, :]/camera_projection[2, :]
    projections_2d[1, :] = camera_projection[1, :]/camera_projection[2, :]

    return projections_2d.T

    

def error(x):
    global points,pixels
    transform = get_transform(x[0],x[1],x[2],x[3],x[4],x[5])
    points_transfered = apply_transform(points, transform)
    internal_calibration = camera_intrinsic(x[6],x[7],x[8],x[9])
    projected = project(points_transfered,internal_calibration)
    # print(((projected-pixels)**2).mean())
    return ((projected-pixels)**2).mean()


def generate(points, x):

    transform = get_transform(x[0],x[1],x[2],x[3],x[4],x[5])
    points_transfered = apply_transform(points, transform)
    internal_calibration = camera_intrinsic(x[6],x[7],x[8],x[9])
    projected = project(points_transfered,internal_calibration)
    return projected


points = np.random.rand(100,3)
x_initial = np.random.rand(10)
pixels = generate(points,x_initial)
x_guess = np.random.rand(10)
results = minimize(error,x_guess, method='nelder-mead', tol = 1e-15)
x = results.x
print(x_initial)
print(x)



from Scipy optimize unable to find the correct results

No comments:

Post a Comment