How do I minimize a function where some of the variables are dependent on other variables?

Question

Consider a camera placed in $(XC,YC,ZC)$. The camera is looking down on a flat table $Z=0$:

Introduce a local coordinate system for the camera: $(x,y,z)$. Initially the axes of the local coordinate system for the camera are aligned with the main coordinate system: $(X,Y,Z)$.

Unfortunately during mounting the camera becomes slightly rotated. As a result a vector described in the camera coordinate system: $\vec{dir}$ is expressed as $\vec{\mathit{DIR}}$ the main coordinate system:

$$ \vec{\mathit{DIR}} = R \vec{\mathit{dir}} $$

where R is a rotation matrix. Describing R as a rotation with the angle $\omega$ around the unit vector $[kx,ky,kz]$ I find from Rodrigues' rotation formula: $$ R=\left[\begin{matrix} kx^2(1-\cos(\omega)) + \cos(\omega) & kx ky \left(1 - \cos{\left(\omega \right)}\right) - kz \sin{\left(\omega \right)} & kx kz \left(1 - \cos{\left(\omega \right)}\right) + ky \sin{\left(\omega \right)}\\kx ky \left(1 - \cos{\left(\omega \right)}\right) + kz \sin{\left(\omega \right)} & ky^2(1-\cos(\omega)) + \cos(\omega) & - kx \sin{\left(\omega \right)} + ky kz \left(1 - \cos{\left(\omega \right)}\right)\\kx kz \left(1 - \cos{\left(\omega \right)}\right) - ky \sin{\left(\omega \right)} & kx \sin{\left(\omega \right)} + ky kz \left(1 - \cos{\left(\omega \right)}\right) & kz^2(1-\cos(\omega)) + \cos(\omega) \end{matrix}\right] $$

Now the camera is a line camera (1D). Each pixel in the camera, i, corresponds to an angle $\theta_i$.

A ray from the i'th pixel in the camera has the direction (I am here using the principle of reversibility of light from optics): $$ \vec{\mathit{dir}} = [0,\sin(\theta_i),-\cos(\theta_i)] $$ in the camera coordinate system.

The equation for a ray from the camera is: $$ (X,Y,Z) = (XC,YC,ZC) + s \cdot \vec{\mathit{DIR}} $$ where $s \ge 0$.

The ray intersect the table at $Z=0$: $$ 0 = ZC + s \cdot \mathit{DIR}_Z $$ where $\mathit{DIR}_Z$ is the $z$ component of the $\vec{\mathit{DIR}}$. Solve this for s: $$ s = -\frac{ZC}{\mathit{DIR}_Z} $$ Substitute this into the equation for $X$ and $Y$: $$ X = XC + \alpha ZC $$ $$ Y = YC + \beta ZC $$ where I have introduced $$ \alpha = -\frac{\mathit{DIR}_X}{\mathit{DIR}_Z} $$ $$ \beta = -\frac{\mathit{DIR}_Y}{\mathit{DIR}_Z} $$

I now have a forward model that calculates a position in $R^2$: $(X,Y)$. The model uses 7 parameters that I need to estimate: $$\vec{C}=[XC,YC,ZC,kx,ky,kz,\omega]$$

I have $N$ measurements: $\{(\theta_i,X_i,Y_i)\}$. From these measurements I want to estimate $\vec{C}$.

I have made a Square Error expression involving the measurements and $\vec{C}$:

$$ SE = \frac{1}{2N}\sum_{i=1}^{N} [(XC - Xi + \alpha\ ZC))^2 + (YC - Yi + \beta ZC)^2] $$

I can minimize this expression eg. using scipy.optimize.minimize().

However there is one "snag" that I do not understand the consequences of; the length of the rotation vector must be one: $$ kx^2+ky^2+kz^2=1. $$ If I solve for $\vec{C}$ I could end up with a $\vec{k}$ with a norm different than 1. Could I then simply divide the found rotation by the length of the k vector: $$ \omega_\text{effective} = {\omega \over (\sqrt{kx^2+ky^2+kz^2})} \;? $$ Alternatively I could perhaps express $kx$, $ky$ and $kz$ in spherical coordinates before minimizing?

For instance: $$ \begin{align} kx &= \sin(\phi)\cos(\psi)\\ ky &= \sin(\phi)\sin(\psi)\\ kz &= \cos(\phi) \end{align} $$

As answered by Cesareo I could use constraints with scipy.optimize.minimize().

However does this reduce the possible algorithms I could use (to e.g. SLSQP)?

Answer 1

In another scenario, if you want to find the rotation matrix that specifies the orientation of the camera in space, then the vector $\{ \theta_i \}$ is not sufficient. In addition to $\theta_i$, you need to have an angle $ \phi_i $ that specifies the polar angle with which point $P_i$ was observed at the camera. If $R$ is the rotation matrix, and $C$ is the location of the camera, then

$ P_i = C + R \| P_i - C \| u_i $

where $u_i = ( \sin \theta_i \cos \phi_i , \sin \theta_i \sin \phi_i, \cos \theta_i ) $

and

$ \| P_i - C \| = \sqrt{ (P_i - C)^T (P_i - C) } $

We now define our error function as follows

$ E(C, R) = \displaystyle \sum_i (P_i - C - R \| P_i - C \| u_i )^T (P_i - C - R \| P_i - C \| u_i ) $

Expanding the above expression, and using $u_i^T u_i = 1 $ ($u_i$ is a unit vector ), and $R^T R = I $, we get

$ E(C, R) = \displaystyle \sum_i \bigg( 2 P_i^T P_i + 2 C^T C - 4 P_i^T C - 2 P_i^T R \| P_i - C \| u_i + 2 C^T R \| P_i - C \| u_i \bigg)$

The gradient of this function with respect to $C$ is

$\nabla_C (E) = \displaystyle \sum_i \bigg( 4 C - 4 P_i + 2 \dfrac{(P_i - C)^T R u_i}{\| P_i - C \| } (P_i - C) + 2 \| P_i - C \| R u_i \bigg) $

In addition, we have the matrix $R$, which instead of specifying using an axis of rotation and an angle of rotation, can be specified as the composition of three consecutive rotations, with angles $ \theta , \phi, \beta $ respectively, as follows

$ R = R_z( \phi ) R_y(\theta) R_z( \beta) $

where $R_z$ is the rotation matrix about the $z$ axis, and $R_y$ is the rotation matrix about the $y$ axis by the given angle.

So now our unknowns vector is $X = [C_x, C_y, C_z, \theta, \phi, \beta ]^T $

Computation of the gradient of $E$ with respect to $\theta, \phi, \beta$ is straight forward and it is as follows

$ \dfrac{\partial E}{\partial \theta} = \displaystyle \sum_i \bigg(-2 \| P_i - C \| (P_i - C) ^T \dfrac{\partial R}{\partial \theta} u_i \bigg) $

Similarly,

$ \dfrac{\partial E}{\partial \phi} = \displaystyle \sum_i \bigg(-2 \| P_i - C \| (P_i - C) ^T \dfrac{\partial R}{\partial \phi} u_i \bigg) $

$ \dfrac{\partial E}{\partial \beta} = \displaystyle \sum_i \bigg(-2 \| P_i - C \| (P_i - C) ^T \dfrac{\partial R}{\partial \beta} u_i \bigg) $

And, to evaluate these partial derivatives of the rotation matrix, we have

$ \dfrac{\partial R}{\partial \theta} = R_z(\phi) \dfrac{\partial R_y}{\partial \theta} R_z(\beta) $

$ \dfrac{\partial R}{\partial \phi} = \dfrac{\partial R_z}{\partial \phi} R_y(\theta) R_z(\beta) $

$ \dfrac{\partial R}{\partial \beta} = R_z(\phi) R_y(\theta) \dfrac{\partial R_z}{\partial \beta} $

That's all the formulation we need, the rest is applying optimization that utilizes the gradient information. This is what I did by using the steepest descent method, and it converged successfully to the true values of $C$ and $R$ in the simulation.

The code for minimization using the steepest descent is found in my other answer, which can be used here with obvious modifications.

Answer 2

Given a set of points $\{ P_i , \theta_i, i = 1, 2 , \dots, n \}$, you want to find the center of the camera $C = (C_x, C_y, C_z)$ and its direction unit vector $a = (a_x, a_y, a_z) $. Ideally, we have,

$ \theta_i = \cos^{-1} \bigg( \dfrac{ a \cdot (P_i - C) } { \| P_i - C \| } \bigg)$

i.e.

$ \cos(\theta_i) \| Pi - C \| = a \cdot (P_i - C) $

Squaring, and re-arranging, this becomes,

$ (P_i - C)^T ( \cos^2(\theta_i) I_3 - {a a} ^T ) (P_i - C) = 0 $

Since $ a $ and $C$ are not known, the above expression will be different from $0$. Therefore, we will define our error function as follows

$ E(C, a) = \displaystyle \sum_{i=1}^n \varepsilon_i^2 $

where

$ \varepsilon_i = (P_i - C)^T ( \cos^2(\theta_i) I_3 - {a a} ^T ) (P_i - C) $

Now we can minimize $E(C, a)$ using any optimization method. I used the simple and basic steepest descent method. For that I need to compute the gradient vector of $E(C, a)$. First, I expressed the unit vector using spherical coordinates

$ a = ( \sin \theta \cos \phi , \sin \theta \sin \phi, \cos \theta ) $

So that my unknowns vector is $X = (C_x, C_y, C_z, \theta, \phi ) $

Now, the gradient of $E(C, a)$ with respect to $C$ is

$ \nabla_C \ E(C, a) = - 2 \displaystyle \sum_{i=1}^n \varepsilon \cdot ( 2 (\cos^2(\theta_i) I_w - {a a}^T ) (P_i - C) ) $

And the partial derivative of $E(C,a)$ with respect to $\theta$ is given by

$ \dfrac{\partial E(C,a) }{\partial \theta} = -2 \displaystyle \sum_{i=1}^n \varepsilon \cdot (2 a_\theta^T (P_i - C)(P_i-C)^T a ) $

where

$ a_\theta = \dfrac{ \partial a }{\partial \theta} = (\cos \theta \cos \phi, \cos \theta \sin \phi, - \sin \theta ) $

Similarly,

$ \dfrac{\partial E(C,a) }{\partial \phi} = -2 \displaystyle \sum_{i=1}^n \varepsilon \cdot (2 a_\phi^T (P_i - C)(P_i-C)^T a ) $

where

$ a_\phi = \dfrac{ \partial a }{\partial \phi} = (-\sin \theta \sin \phi, \sin \theta \cos \phi, 0 ) $

Now, we have all the components of the gradient vector. So we can apply the steepest descent algorithm.

I've implemented this in a simple program, and I got convergence to the true value of the parameters even when the initial estimate of $X$ is set at $45 \%$ of its true value.

Here is a complete listing of my program for reference. It is written in Microsoft Excel VBA Script (Visual Basic for Applications Script).

Public Sub identify_location_and_direction_of_camera()

Dim points(10, 3) As Double
Dim r0(3) As Double
Dim axis(3) As Double

Dim theta(10) As Double
Dim a(3), c(3) As Double
Dim x(5), y(5), jac(5, 5) As Double
Dim x_(5), y_(5), dx(5), dy(5) As Double, gamma As Double

r0(1) = 2
r0(2) = 1
r0(3) = 16

n = 10

For i = 1 To n
     For j = 1 To 3
     points(i, j) = ActiveSheet.Cells(i, j)
     Next j
Next i

axis(1) = 2
axis(2) = 3
axis(3) = -5

Call normalize1(axis)

For i = 1 To n

    For j = 1 To 3
        u1(j) = points(i, j) - r0(j)
    Next j

    theta(i) = WorksheetFunction.Acos(dot(u1(3), axis)/norm(u1,3))
Next i

' Now set X to 45% of its true value.

x(4) = WorksheetFunction.Acos(axis(3))
x(5) = WorksheetFunction.Atan2(axis(1), axis(2))

For i = 1 To 3
    x(i) = r0(i)
Next i

For i = 1 To 3
    x(i) = 0.45 * x(i)
Next i

success = 0

For ic = 1 To 1000

Call find_y_camera(n, points, theta, x, y, a, c, esq)

If norm(y, 5) < 0.0000001 Then
   success = 1
   MsgBox ("Converged in " + Str(ic - 1))
   Exit For
End If

If ic = 1 Then
   gamma = 0.01
Else
   For i = 1 To 5
      dx(i) = x(i) - x_(i)
      dy(i) = y(i) - y_(i)
   Next i

   If norm(dx, 5) < 0.000000001 Then
      success = 1
      MsgBox ("converged in " + Str(ic - 1))
      Exit For
   End If

   dynormsq = norm(dy, 5) ^ 2

   gamma = Abs(dotn(dx, dy, 5)) / dynormsq

End If


For i = 1 To 5
   x_(i) = x(i)
   x(i) = x(i) - gamma * y(i)
   y_(i) = y(i)
Next i

Next ic

If success = 0 Then
   MsgBox ("Did not converge")
   Exit Sub
End If

For i = 1 To 3
    ActiveSheet.Cells(15 + i, 1) = c(i)
    ActiveSheet.Cells(15 + i, 2) = r0(i)

    ActiveSheet.Cells(15 + i, 4) = a(i)
    ActiveSheet.Cells(15 + i, 5) = axis(i)
Next i

End Sub

---

Public Sub find_y_camera(n, points, theta, x, y, a, c, esq)
Dim p0(3) As Double
Dim m(3, 3), vec(3) As Double
Dim ppt(3, 3) As Double
Dim e As Double
Dim ath(3), aphi(3) As Double

esq = 0

For i = 1 To 5
   y(i) = 0
Next i


For i = 1 To 3
   c(i) = x(i)
Next i

th = x(4)
phi = x(5)

a(1) = Sin(th) * Cos(phi)
a(2) = Sin(th) * Sin(phi)
a(3) = Cos(th)

ath(1) = Cos(th) * Cos(phi)
ath(2) = Cos(th) * Sin(phi)
ath(3) = -Sin(th)

aphi(1) = -Sin(th) * Sin(phi)
aphi(2) = Sin(th) * Cos(phi)
aphi(3) = 0

For ipoint = 1 To n

    t = theta(ipoint)

    For j = 1 To 3
        p0(j) = points(ipoint, j) - c(j)
    Next j

For i = 1 To 3
    For j = 1 To 3

        If i = j Then 
           m(i, j) = Cos(t) ^ 2 - a(i) * a(j) 
        Else 
           m(i, j) = -a(i) * a(j)
        End If
        
     Next j
Next i

e = xtqy(3, m, p0, p0)

Call multiplyv(3, 3, m, p0, vec)
Call vscale(3, vec, -e, vec)

For i = 1 To 3
    y(i) = y(i) + vec(i)
Next i

' now calculate y(4:5)

For i = 1 To 3
    For j = 1 To 3
        ppt(i, j) = p0(i) * p0(j)
    Next j
Next i

y(4) = y(4) - e * xtqy(3, ppt, ath, a)
y(5) = y(5) - e * xtqy(3, ppt, aphi, a)

esq = esq + e ^ 2

Next ipoint

End Sub

---

Public Function dot(x, y) As Double

dot = x(1) * y(1) + x(2) * y(2) + x(3) * y(3)

End Function

---

Public Function dotn(x, y, n) As Double
Dim s As Double

s = 0

For i = 1 To n
    s = s + x(i) * y(i)
Next i

dotn = s

End Function

---

Public Sub multiply(n, m, p, a, b, c)
Dim i, j, k As Integer
Dim c1(200, 200) As Double

For i = 1 To n
    For j = 1 To p
        c1(i, j) = 0
        For k = 1 To m
            c1(i, j) = c1(i, j) + a(i, k) * b(k, j)
        Next k
    Next j
Next i

For i = 1 To n
    For j = 1 To p
        c(i, j) = c1(i, j)
    Next j
Next i

End Sub

---

Public Sub multiplyv(n, m, a, b, c)
Dim i, j, k As Integer
Dim c1(200) As Double

For i = 1 To n
    c1(i) = 0
    For k = 1 To m
        c1(i) = c1(i) + a(i, k) * b(k)
    Next k
Next i

For i = 1 To n
    c(i) = c1(i)
Next i

End Sub

---

Public Sub vscale(ByVal n, x, a, y)

For i = 1 To n
    y(i) = a * x(i)
Next i


End Sub

---

Public Sub multiplyrv(n, m, a, b, c)
Dim i, j, k As Integer
Dim c1(100) As Double

For i = 1 To m
    c1(i) = 0
    For k = 1 To n
        c1(i) = c1(i) + a(k) * b(k, i)
    Next k
Next i

For i = 1 To m
    c(i) = c1(i)
Next i

End Sub

---

Public Function xtqy(n, q, x, y) As Double
Dim temp(10) As Double
Dim a As Double

Call multiplyrv(n, n, x, q, temp)

a = 0
For i = 1 To n
    a = a + temp(i) * y(i)
Next i

xtqy = a

End Function

---