読者です 読者をやめる 読者になる 読者になる

Gauss Newton 法,Levenberg-Marquardt 法 in Python

ちょっと前回の投稿からアップが遅れてしまいましたが,さぼっていたわけではなく...
引き続き Teb Planner のパラメータ調整と解析をしてたんですが,もう少し詳しく理解したかったんでソースを読んでみました.
で,気づいたんですが,LSD SLAM にしても, Teb Planner にしても,バンドル調整にしても,最近よく出てくる(少なくとも自分の周りで)技術は,結局非線形方程式のパラメータ最適化をしているなと.
で,この非線形方程式の最適化なんですが,あまりちゃんと勉強したことが今までなかったのでちょっとハードルが高かったのですが,せっかくの機会なのでちょっとかじってみることにしました.

読んだ本は,有名な下記の本です.さすが有名なだけあって,わかりやすかったですね.

これなら分かる最適化数学―基礎原理から計算手法まで

これなら分かる最適化数学―基礎原理から計算手法まで

なかなかちゃんと理解するのに時間がかかったんですが,根本的なアイデアとしては一次元のニュートン法が出発点になるんですね.
でも,高次元になるとヘッセ行列とかがいっぱい出てきて,途端に難しくなります...下記,簡単な例ですが,Python非線形方程式のパラメータ推定をやってみました.

1. Gauss Newton 法

import numpy as np
import numpy.linalg as linalg
import random

# Function is 
# f(x, y) = u0*x^3 + u1*x^2*(y^2 + u2*y)
def f(x, y, z, u):
    return u[0] * x**3 + u[1] * x**2 * (y**2 + u[2] * y) - z
    
def df_du0(x, y, u):
    return x**3
    
def df_du1(x, y, u):
    return x**2 * (y**2 + u[2] * y)
    
def df_du2(x, y, u):
    return u[1] * x**2 * y

def calculateDeltaU(H_sum, J_sum):
    return linalg.lstsq(H_sum, J_sum)

def jacobian(x, y, z, u):
    nablaF = np.array([[df_du0(x, y, u)], [df_du1(x, y, u)], [df_du2(x, y, u)]])
    return f(x, y, z, u) * nablaF

def hessian(x, y, u):
    nablaF = np.array([[df_du0(x, y, u)], [df_du1(x, y, u)], [df_du2(x, y, u)]])
    return nablaF * nablaF.T

def createDataSet(x, y, u):
    z = []
    for i in range(len(x)):
        noise = random.random() * 0.01
        z.append(f(x[i], y[i], 0, u) + noise)
    return z

def gaussNewton(x, y, z, u0, eps):
    
    u = np.array(u0).reshape((3, 1)).astype(float)
    oldDeltaU = np.zeros((3, 1))
    deltaU = np.zeros((3, 1))
    
    while(True):

        J_sum = np.zeros((3, 1))
        H_sum = np.zeros((3, 3))
    
        for i in range(len(x)):
            H_sum += hessian(x[i], y[i], u)
            J_sum += jacobian(x[i], y[i], z[i], u)
        
        oldDeltaU = deltaU
        deltaU = calculateDeltaU(H_sum, J_sum)[0]
        u -= deltaU 
    
        if (linalg.norm(deltaU[0] - oldDeltaU) < eps):
            break
    
    return u

if __name__ == "__main__":
    
    trueU = [0.8, 0.8, 0.8]
    u0 = [1.5, 1.5, 15]
    x = [1 for i in range(10)]
    y = [i for i in range(10)]
    z = createDataSet(x, y, trueU)         
    
    u = gaussNewton(x, y, z, u0, 0.00001)
    print("Estimated u : ", u)
    
2. Levenberg-Marquardt
# -*- coding: utf-8 -*-


import numpy as np
import numpy.linalg as linalg
import random

# Function is 
# f(x, y) = u0*x^3 + u1*x^2*(y^2 + u2*y)
def f(x, y, z, u):
    return u[0] * x**3 + u[1] * x**2 * (y**2 + u[2] * y) - z

def J(x, y, z, u):
    totalJ = 0
    for i in range(len(x)):
        totalJ += (f(x[i], y[i], z[i], u))**2
    return 0.5 * totalJ    

def df_du0(x, y, u):
    return x**3
    
def df_du1(x, y, u):
    return x**2 * (y**2 + u[2] * y)
    
def df_du2(x, y, u):
    return u[1] * x**2 * y

def calculateDeltaU(H_sum, J_sum):
    return linalg.lstsq(H_sum, J_sum)

def jacobian(x, y, z, u):
    nablaF = np.array([[df_du0(x, y, u)], [df_du1(x, y, u)], [df_du2(x, y, u)]])
    return f(x, y, z, u) * nablaF

def hessian(x, y, u):
    nablaF = np.array([[df_du0(x, y, u)], [df_du1(x, y, u)], [df_du2(x, y, u)]])
    return nablaF * nablaF.T

def createDataSet(x, y, u):
    z = []
    for i in range(len(x)):
        noise = random.random() * 0.01
        z.append(f(x[i], y[i], 0, u) + noise)
        #z.append(f(x[i], y[i], 0, u))
    return z

def levenbergMarquardt(x, y, z, u0, eps):
    
    c = 0.0001
    u = np.array(u0).reshape((3, 1)).astype(float)
    tmpu = u
    oldDeltaU = np.zeros((3, 1))
    deltaU = np.zeros((3, 1))
    newJ = J(x, y, z, u0)
    oldJ = newJ
    
    while(True):

        J_sum = np.zeros((3, 1))
        H_sum = np.zeros((3, 3))
    
        for i in range(len(x)):
            H_sum += hessian(x[i], y[i], u)
            J_sum += jacobian(x[i], y[i], z[i], u)
        
        H_sum = H_sum + c * H_sum * np.identity(3)             
        tmpDeltaU = calculateDeltaU(H_sum, J_sum)[0]
        tmpu -= tmpDeltaU    
        oldJ = newJ
        newJ = J(x, y, z, u)
        
        if (newJ > oldJ):
            c = 10 * c
            tmpu = u
            tmpDeltaU = deltaU
        else:
            c = 0.1 * c
            u = tmpu
            oldDeltaU = deltaU  
            deltaU = tmpDeltaU

        if (linalg.norm(deltaU[0] - oldDeltaU) < eps):
            break
    
    return u, newJ

if __name__ == "__main__":
    
    trueU = [0.8, 0.8, 0.8]
    u0 = [10, 10, 20]
    x = [1 for i in range(10)]
    y = [0.5 * i for i in range(10)]
    z = createDataSet(x, y, trueU)         
    
    u, Jval = levenbergMarquardt(x, y, z, u0, 0.000001)
    print("Estimated u : {0}, J : {1}".format(u, Jval))