work on calib

src/calibNAXP.jl

@@ -154,23 +154,25 @@ function calibNAXP2(points_S, lines_S, POSES, T_TF_S, planes::AbstractVector{Int
             N0     = [Nt; 0]
             Ps     = [points_S[:,i]; 1]
             Pr     = points[:,i]
-            y[i]   = -N0⋅(Ps - trinv(Tr)*Pr)
-            a      = -skew(Nt)*[Ps[1:3] (trinv(Tr)*Pr)[1:3]]
-            A[i,:] = [a[:,1]+a[:,2]; 2Nt]
+            y[i]   = -N0⋅(trinv(Tr)*(Ps - N0))
+            a      = -skew(Nt)*((trinv(Tr)*(Ps+N0))[1:3])
+            A[i,:] = [a; Nt]
             if true # Add point from line
                 Ps = [points_S[:,i] + 0.1*1.01^c*randn()*lines_S[:,i]; 1]
-                y[i+N_poses] = -N0⋅(Ps - trinv(Tr)*Pr)
-                a = -skew(Nt)*[Ps[1:3] (trinv(Tr)*Pr)[1:3]]
-                A[i+N_poses,:] = [a[:,1]+a[:,2]; 2Nt]
+                y[i+N_poses] = -N0⋅(trinv(Tr)*(Ps - N0))
+                a      = -skew(Nt)*((trinv(Tr)*(Ps+N0))[1:3])
+                A[i+N_poses,:] = [a; Nt]
             end
 
         end
         w   = A\y
         er  = y-A*w
         # @show sum(abs2.(er))
         S = [skew(w[1:3]) w[4:6]; [0 0 0 0]]
+        # display(T_TF_S)
         T_TF_S = (I-S)\(I+S)
-        # @show T_TF_S - expξ([w[4:6]; w[1:3]])
+        # display.((T_TF_S, w))
+        # error()
 
         RMSi = zeros(N_planes)
         for j = 1:N_planes

src/symbolic_ls.jl

@@ -94,25 +94,25 @@ w = [g;u][:]
 # Prb = A*inv(I4-S)*(I4+S)*Ps
 
 
-eq1 = N0'*[G u;0 0 0 0]*(Ps + trinv(A)*Prb)
+eq1 = N0'*S*trinv(A)*(Ps + N0)
 
 eq = eq1[:] # Vector of interesting equations,
-# terms of order zeros should be placed on RHS
 
-AA     = Array{Sym}(length(eq), length(w))
+AA     = Array{Sym}(length(w))
 BB     = Array{Sym}(length(eq), 1)
-AAz    = falses(size(AA))
+AA2    = similar(AA)
 
-for i = 1:length(eq)
-    for j = 1:length(w)
-        AA[i,j] = coeffs(expand(eq[i]),w[j])[2] # Find all coefficients of parameter j in eq i
-    end
+
+for j = 1:length(w)
+    AA[j] = coeffs(expand(eq[i]),w[j])[1]
+    AA2[j] = coeffs(expand(eq[i]),w[j])[2]
 end
 
-# byhand = (reshape(-repmat((N'A[1:3,1:3]),3,1)',9,1) .*[reshape(repmat(P[1:2],1,3)',6,1);2N])
-byhand2 = -skew(N)*[Ps[1:3] (trinv(A)*Prb)[1:3]]
-byhand2 = sum(byhand2, dims=2)
-byhand2 = [byhand2; 2N]
+
+# TODO: don't forget to extract potential coefficients that does not have a match in AA
+
+byhand2 = -skew(N)*((trinv(A)*(Ps+N0))[1:3])
+byhand2 = [byhand2; N]
 # @assert all(simplify.(byhand) .== simplify.(byhand2[:]))
 @assert all(simplify.(byhand2 .- AA[:]) .== 0)
 
@@ -122,3 +122,23 @@ r = map(AA) do a
     end
 end
 r = hcat(r...)
+
+
+
+
+p = randn(3); n = randn(3); T = randn(3,3)
+p *= (n'n)/(n'p)
+n'n - n'p
+
+n'T*p
+trace(T*p*n')
+
+(T*p)⋅n
+((T*n)⋅n)
+
+n'T*p
+n'T*n
+
+trace(n*p'*T')
+
+Pn = n*n'

test/testCalibNAXP.jl

@@ -135,6 +135,7 @@ function run_calib()
         T_TF_S_real = copy(T_TF_S)
         if true
             T_TF_S0 = Rt2T(rpy2R(60π/180*(rand(3)-0.5)),0.4*(rand(3)-0.5))*T_TF_S_real
+            # T_TF_S0 = Rt2T(rpy2R(10π/180*(rand(3)-0.5)),0.1*(rand(3)-0.5))*T_TF_S_real
             #         T_TF_S0 = rt2tr(rpy2R(2*(rand(1,3)-0.5),'deg'),0.01*(rand(3,1)-0.5))*T_TF_S_real;
         else
             T_TF_S0 = copy(T_TF_S_real)
@@ -149,7 +150,7 @@ function run_calib()
         normStart[mc] = vecnorm(T_TF_S0-T_TF_S_real)
         distStart[mc] = norm(T_TF_S0[1:3,4]-T_TF_S_real[1:3,4])
         rotStart[mc] = norm(180/π*R2rpy(T_TF_S0\T_TF_S_real))
-
+        # display(T_TF_S_real)
         T_TF_S, RMScalibs,ALLcalibs, norms = calibNAXP2(points_S, lines_S, T_RB_TF, T_TF_S0, planes,  iters)#, T_TF_S_real)
 
         SSEMC[mc,:] = RMScalibs