% determines residuals etc. for 2D homography from point pairs with minimal representation [lr,cg,Crrt,atr,btr] = sugr_ghm_cg_2D_homography_from_point_pairs(l,le,xe,Crro) * l = 6 x 1 vector, observed point pair * le = 6 x 1 vector, approximated fitted point pair * xe = 3 x 3 matrix, approximated homography * Crro = 4 x 4 matrix, reduced covariance matrix of point pair * lr = 4 x 1 vector, reduced observation * cg = 2 x 1 vector, residual of constraint * Crrt = 4 x 4 matrix, reduced covariance matrix of transformed observations * atr = 2 x 8 vector, transposed Jacobian for cg -> xe * btr = 2 x 4 vector, transposed jacobian for cg -> le 0 != S^(l)(y) H x = -S^(l)(H x) y = (x' cron S^(l)(y)) vec(H) Wolfgang Förstner wfoerstn@uni-bonn.de wf 03/2011
0001 %% determines residuals etc. for 2D homography from point pairs 0002 % with minimal representation 0003 % 0004 % [lr,cg,Crrt,atr,btr] = sugr_ghm_cg_2D_homography_from_point_pairs(l,le,xe,Crro) 0005 % 0006 % * l = 6 x 1 vector, observed point pair 0007 % * le = 6 x 1 vector, approximated fitted point pair 0008 % * xe = 3 x 3 matrix, approximated homography 0009 % * Crro = 4 x 4 matrix, reduced covariance matrix of point pair 0010 % 0011 % * lr = 4 x 1 vector, reduced observation 0012 % * cg = 2 x 1 vector, residual of constraint 0013 % * Crrt = 4 x 4 matrix, reduced covariance matrix of transformed observations 0014 % * atr = 2 x 8 vector, transposed Jacobian for cg -> xe 0015 % * btr = 2 x 4 vector, transposed jacobian for cg -> le 0016 % 0017 % 0 != S^(l)(y) H x = -S^(l)(H x) y = (x' cron S^(l)(y)) vec(H) 0018 % 0019 % Wolfgang Förstner 0020 % wfoerstn@uni-bonn.de 0021 % 0022 % wf 03/2011 0023 0024 function [lr,Cr,cg,atr,btr] = sugr_ghm_cg_2D_homography_from_point_pairs(l,le,xe,C) 0025 0026 % Jacobian for observation -> reduced observation: 6 x 4 matrix 0027 Jle = [null(le(1:3)') zeros(3,2);... 0028 zeros(3,2) null(le(4:6)')] ; 0029 0030 % reduced observation_ 4 x 1 vector 0031 lr = Jle' * l; 0032 0033 % Rotation from l to le: 6 x 6 matrix 0034 R = [calc_Rot_ab(le(1:3),l(1:3)) zeros(3);... 0035 zeros(3) calc_Rot_ab(le(4:6),l(4:6))]; 0036 0037 % Jacobian for Cr 0038 JR = Jle' * R' * [null(le(1:3)') zeros(3,2);... 0039 zeros(3,2) null(le(4:6)')] ; 0040 0041 % trasnferred covariance matrix 0042 Cr = JR * C * JR'; 0043 0044 % determine selected constraints 0045 [Ssy, Rsy] = calc_S_reduced(le(4:6)); 0046 0047 % Jacobian for cg -> x 0048 Jhr = sugr_get_Jacobian_Jhr_Homography_2D(xe); 0049 atr = kron(le(1:3)', Ssy) * Jhr; % 2 x 8 Matrix 0050 0051 % Jacobain for cg -> l 0052 btr = [Ssy * xe * null(le(1:3)') , ... 0053 -Rsy * calc_S(xe * le(1:3)) * null(le(4:6)')] ; % 2 x 4 matrix 0054 0055 % residual of constraint (lr = -vr) 0056 cg = - Ssy * xe * le(1:3) - btr * lr; 0057