MLEupdate.m

function [ m_mle ] = MLEupdate( T, m )
% This function computes the MLE correction to the Trifocal Tensor 
%   Algorithm can be found in section B from https://www.robots.ox.ac.uk/~vgg/publications/1997/Torr97a/torr97a.pdf
    
    J = zeros(4,3);
    J(1,1) = T(1,1,1)-T(1,1,3)*m.a3(1)-T(1,3,1)*m.a2(1)+T(1,3,3)*m.a2(1)*m.a3(1);
    J(1,2) = -m.a1(1)*T(1,3,1)+m.a1(1)*m.a3(1)*T(1,3,3);
    J(1,3) = -m.a1(1)*T(1,1,3)+m.a1(1)*m.a2(1)*T(1,3,3);
    J(2,1) = T(1,2,1)-m.a3(1)*T(1,2,3)-m.a2(2)*T(1,3,1)+m.a2(2)*m.a3(1)*T(1,3,3);
    J(2,2) = 0;
    J(2,3) = -m.a1(1)*T(1,2,3)+m.a1(1)*m.a2(2)*T(1,3,3);
    J(3,1) = T(1,1,2)-m.a3(2)*T(1,1,3)-m.a2(1)*T(1,3,3)+m.a2(1)*m.a3(2)*T(1,3,3);
    J(3,2) = -m.a1(1)*T(1,3,3)+m.a1(1)*m.a3(2)*T(1,3,3);
    J(3,3) = 0;
    J(4,1) = T(1,2,2)-m.a3(2)*T(1,2,3)-m.a2(2)*T(1,3,2)+m.a2(2)*m.a3(2)*T(1,3,3);
    J(4,2) = 0;
    J(4,3) = 0;
    
    r = zeros(4,1);
    r(1) = m.a1(1)*T(1,1,1)+m.a1(2)*T(2,1,1)+m.a1(3)*T(3,1,1) ...
          -m.a1(1)*m.a3(1)*T(1,1,3)-m.a1(2)*m.a3(1)*T(2,1,3)-m.a1(3)*m.a3(1)*T(3,1,3) ...
          -m.a1(1)*m.a2(1)*T(1,3,1)-m.a1(2)*m.a2(1)*T(2,3,1)-m.a1(3)*m.a2(1)*T(3,3,1) ...
          +m.a1(1)*m.a2(1)*m.a3(1)*T(1,3,3)+m.a1(2)*m.a2(1)*m.a3(1)*T(2,3,3)+m.a1(3)*m.a2(1)*m.a3(1)*T(3,3,3);
    r(2) = m.a1(1)*T(1,2,1)+m.a1(2)*T(2,2,1)+m.a1(3)*T(3,2,1) ...
          -m.a1(1)*m.a3(1)*T(1,2,3)-m.a1(2)*m.a3(1)*T(2,2,3)-m.a1(3)*m.a3(1)*T(3,2,3) ...
          -m.a1(1)*m.a2(2)*T(1,3,1)-m.a1(2)*m.a2(2)*T(2,3,1)-m.a1(3)*m.a2(2)*T(3,3,1) ...
          +m.a1(1)*m.a2(2)*m.a3(1)*T(1,3,3)+m.a1(2)*m.a2(2)*m.a3(1)*T(2,3,3)+m.a1(3)*m.a2(2)*m.a3(1)*T(3,3,3);
    r(3) = m.a1(1)*T(1,1,2)+m.a1(2)*T(2,1,2)+m.a1(3)*T(3,1,2) ...
          -m.a1(1)*m.a3(2)*T(1,1,3)-m.a1(2)*m.a3(2)*T(2,1,3)-m.a1(3)*m.a3(2)*T(3,1,3) ...
          -m.a1(1)*m.a2(1)*T(1,3,2)-m.a1(2)*m.a2(1)*T(2,3,2)-m.a1(3)*m.a2(1)*T(3,3,2) ...
          +m.a1(1)*m.a2(1)*m.a3(2)*T(1,3,3)+m.a1(2)*m.a2(1)*m.a3(2)*T(2,3,3)+m.a1(3)*m.a2(1)*m.a3(2)*T(3,3,3);
    r(4) = m.a1(1)*T(1,2,2)+m.a1(2)*T(2,2,2)+m.a1(3)*T(3,2,2) ...
          -m.a1(1)*m.a3(2)*T(1,2,3)-m.a1(2)*m.a3(2)*T(2,2,3)-m.a1(3)*m.a3(2)*T(3,2,3) ...
          -m.a1(1)*m.a2(2)*T(1,3,2)-m.a1(2)*m.a2(2)*T(2,3,2)-m.a1(3)*m.a2(2)*T(3,3,2) ...
          +m.a1(1)*m.a2(2)*m.a3(2)*T(1,3,3)+m.a1(2)*m.a2(2)*m.a3(2)*T(2,3,3)+m.a1(3)*m.a2(2)*m.a3(2)*T(3,3,3);
    
    mchange = -pinv(J)*r;
    m_mle = m;
    m_mle.a1(1) = m.a1(1) + mchange(1);
    m_mle.a2(1) = m.a2(1) + mchange(2);
    m_mle.a3(1) = m.a3(1) + mchange(3);
end