'以下函数参考:Avishek Chatterjee and Venu Madhav Govindu .Robust Relative Rotation Averaging
    Private Function GemanMcClureFun(ByRef x As Double) As Double
        Const a2 As Double = 10.0
        Dim x2 As Double = x * x
        Dim sum As Double = a2 + x2
        Return x2 / (sum + sum)
    End Function
    Private Function GemanMcClureDFun(ByRef x As Double) As Double
        Const a2 As Double = 10.0
        Dim x2 As Double = x * x + a2
        Return a2 * x / (x2 * x2)
    End Function

    Private Function PseudoHuberFun(ByRef x As Double) As Double
        Const a2 As Double = 1.0 / 10.0
        Return sqrt(1.0 + x * x * a2) - 1.0
    End Function
    Private Function PseudoHuberDFun(ByRef x As Double) As Double
        Const a2 As Double = 1.0 / 10.0
        Return a2 * x / sqrt(1.0 + x * x * a2)
    End Function

    Private Function AndrewsFun(ByRef x As Double) As Double
        Const a As Double = 10.0
        Const a2 As Double = a * a
        Const api As Double = a * 3.14159265358979
        Const a22 as Double = a2 + a2
        Const a1 = 1 / a
        If abs(x) <= api Then
            Return a2 * (1.0 - Cos(x * a1))
        Else
            Return a22
        End If
    End Function
    Private Function AndrewsDFun(ByRef x As Double) As Double
        Const a As Double = 10.0
        Const a2 As Double = a * a
        Const api As Double = a * 3.14159265358979
        Const a1 = 1 / a
        If abs(x) <= api Then
            Return a * sin(x * a1)
        Else
            Return 0.0
        End If
    End Function

    Private Function TukeyBiFun(ByRef x As Double) As Double
        Const a As Double = 10.0
        Const a2 As Double = a * a / 6
        Const a1 As Double = 1 / a
        Dim z As Double
        If abs(x) <= a Then
            z = x * a1
            z = 1.0 - z * z
            z = z * z * z
            Return a2 * (1 - z)
        End If
        Return 0.0
    End Function
    Private Function TukeyBiDFun(ByRef x As Double) As Double
        Const a As Double = 10.0
        Const a1 As Double = 1 / a
        Dim z As Double
        If abs(x) <= a Then
            z = x * a1
            z = 1.0 - z * z
            Return x * z * z
        End If
        Return 0.0
    End Function

    Private Function CauchyFun(ByRef x As Double) As Double
        Const a As Double = 10.0
        Const a1 As Double = 1 / a
        Dim z As Double = a1 * x
        z = 1 + z * z
        Return Log(z)
    End Function
    Private Function CauchyDFun(ByRef x As Double) As Double
        Const a As Double = 10.0
        Const a1 As Double = 1 / a
        Const a2 As Double = a1 * (a1 + a1)
        Dim z As Double = a1 * x
        z = 1 + z * z
        Return a2 * x / z
    End Function

    Private Function FairFun(ByRef x As Double) As Double
        Const a As Double = 10.0
        Const a1 As Double = 1 / a
        Const a2 As Double = a * a
        Dim z As Double = abs(x) * a1
        Return a2 * (z - Log(1 + z))
    End Function
    Private Function FairDFun(ByRef x As Double) As Double
        Const a As Double = 10.0
        Const a1 As Double = 1 / a
        Return x / (1 + abs(x) * a1)
    End Function

    Private Function LogisticFun(ByRef x As Double) As Double
        Const a As Double = 10.0
        Const a1 As Double = 1 / a
        Return a * Log(cosh(x * a1))
    End Function
    Private Function LogisticDFun(ByRef x As Double) As Double
        Const a As Double = 10.0
        Const a1 As Double = 1 / a
        Return tanh(x * a1)
    End Function

    Private Function TalwarFun(ByRef x As Double) As Double
        Const a As Double = 10.0
        Const a2 As Double = a * a
        If abs(x) <= a Then
            Return x * x
        Else
            Return a2
        End If
    End Function
    Private Function TalwarDFun(ByRef x As Double) As Double
        Const a As Double = 10.0
        Const a2 As Double = a * a
        If abs(x) <= a Then
            Return x + x
        Else
            Return 0.0
        End If
    End Function

    Private Function WelschFun(ByRef x As Double) As Double
        Const a As Double = 10.0
        Const a2 As Double = a * a / 2
        Const a1 As Double = 1 / a
        Dim z As Double = x * a1
        Return a2 * (1 - exp(-z * z))
    End Function
    Private Function WelschDFun(ByRef x As Double) As Double
        Const a As Double = 10.0
        Const a1 As Double = 1 / a
        Dim z As Double = x * a1
        Return x * exp(-z * z)
    End Function
    Private Function PowFun(ByRef x As Double) As Double
        '|x|^0.5
        If x >= 0.0 Then
            Return sqrt(x) * x
        Else
            Return sqrt(-x) * (-x)
        End If
    End Function
    Private Function PowDFun(ByRef x As Double) As Double
        '|x|^1.5
        If x >= 0.0 Then
            Return sqrt(x) * 1.5
        Else
            Return -sqrt(-x) * 1.5
        End If
    End Function
    Private Function HuberFun(ByRef x As Double) As Double
        Const a As Double = 10.0
        Const a2 as Double = a/2
        if abs(x) <= a Then
           Return x*x/2
        else 
           Return a*(abs(x) - a2)
        end if
    End Function
    Private Function HuberDFun(ByRef x As Double) As Double
        Const a As Double = 10.0
        if abs(x) <= a Then
           Return x
        else 
           Return a*sign(x)
        end if
    End Function
    Private Function CostFun(ByRef x As Double, ByRef index As Integer, ByRef Type_ As Byte) As Double
        'x     为误差平方和
        'index 为第index个样本或者方程
        'Type_ 对应求解器选择索引
        '下面的case 后的数字必须按顺序编写,注释部分会显示在操作面板
        Select Case Type_
            Case 0 'x^2
                Return x * x
            Case 1 '|x|^a
                Return PowFun(x)
            Case 2 'Andrews
                Return AndrewsFun(x)
            Case 3 'Cauchy
                Return CauchyFun(x)
            Case 4 'Fair
                Return FairFun(x)
            Case 5 'Geman-McClure
                Return GemanMcClureFun(x)
            Case 6 'Huber
                Return HuberFun(x)
            Case 7 'Logistic
                Return LogisticFun(x)
            Case 8 'Pseudo-Huber
                Return PseudoHuberFun(x)
            Case 9 'Talwar
                Return TalwarFun(x)
            Case 10 'Tukey Biweight
                Return TukeyBiFun(x)
            Case 11 'Welsch
                Return WelschFun(x)
            Case Else
                Return x * x
        End Select
    End Function
    Private Function CostDFun(ByRef x As Double, ByRef index As Integer, ByRef Type_ As Byte) As Double
        'x     为误差平方和
        'index 为第index个样本或者方程
        'Type_ 对应求解器选择索引
        '本函数为CostFun函数的导数
        Select Case Type_
            Case 0 'x^2
                Return x + x
            Case 1 '|x|^a
                Return PowDFun(x)
            Case 2 'Andrews
                Return AndrewsDFun(x)
            Case 3 'Cauchy
                Return CauchyDFun(x)
            Case 4 'Fair
                Return FairDFun(x)
            Case 5 'Geman-McClure
                Return GemanMcClureDFun(x)
            Case 6 'Huber
                Return HuberDFun(x)
            Case 7 'Logistic
                Return LogisticDFun(x)
            Case 8 'Pseudo-Huber
                Return PseudoHuberDFun(x)
            Case 9 'Talwar
                Return TalwarDFun(x)
            Case 10 'Tukey Biweight
                Return TukeyBiDFun(x)
            Case 11 'Welsch
                Return WelschDFun(x)
            Case Else
                Return x + x
        End Select
    End Function


